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ABSTRACT 

^  In  many  aerospace  problems,  it  is  necessary  to  determine  vehicle  trajectories  that  satisfy 
constraints.  Typically  two  types  of  constraints  are  of  interest.  First,  it  may  be  desirable  to 
satisfy  a  set  of  boundary  conditions.  Second,  it  may  be  necessary  to  limit  the  motion  of  the 
vehicle  so  that  physical  limits  and  hardware  limits  are  not  exceeded.  In  addition  to  these 
requirements,  it  may  be  necessary  to  optimize  some  measure  of  vehicle  performance.  In 
this  thesis,  the  square  root  sweep  method  is  used  to  solve  a  discrete-time  linear  quadratic 
optimal  control  problem.  The  optimal  control  problem  arises  from  a  Mayer  form 
continuous-time  nonlinear  optimization  problem.  A  method  for  solving  the  optimal  control 
problem  is  derived.  Called  the  square  root  sweep  algorithm,  the  solution  consists  of  a  set 
of  backward  recursions  for  a  set  of  square  root  parameters.  The  square  root  sweep 
algorithm  is  shown  to  be  capable  of  treating  Mayer  form  optimization  problems.  Heuristics 
for  obtaining  solutions  are  discussed.  The  square  root  sweep  algorithm  is  used  to  solve 
several  example  optimization  problems.  /  /,  ^  '  ./  /x _ 
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1  Introduction 

1.1  Problem  Motivation 

The  problem  addressed  in  this  thesis  is  constrained  trajectory  generation  and 
optimization.  The  focus  will  be  on  the  development  of  an  efficient  algorithm.  Both 
theoretical  and  computational  aspects  of  the  optimization  problem  are  to  be  addressed. 

These  techniques  are  applicable  to  a  wide  range  of  aerospace  problems.  One 
particular  application  is  the  proposed  National  Aerospace  Plane  (NASP). 
Transatmospheric  vehicles  such  as  the  National  Aerospace  Plane  (NASP)  will  provide  a 
much  more  flexible  space  launch  capability  for  both  civilian  and  military  space  missions. 
Using  a  scramjet  engine,  the  National  Aerospace  Plane  will  takeoff  from  a  conventional 
runway  much  like  today's  commercial  airliners  and  ascend  to  orbit  in  a  single  stage.  The 
National  Aerospace  Plane  will  provide  routine  manned  access  to  space  and  has  the  potential 
of  being  a  true  "orbit  on  demand"  vehicle. 

A  number  of  technical  problems  must  be  solved  before  transatmospheric  vehicles 
become  a  reality.  For  example,  propulsion  systems  capable  of  accelerating  the  vehicle  to 
speeds  near  Mach  25  must  be  developed.  Lightweight,  high  strength  materials  capable  of 
withstanding  high  temperatures  are  required.  Progress  has  been  made  towards  solving 
many  of  these  problems  [1]. 
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Trajectory  optimization  will  play  a  significant  role  in  the  development  of 
transatmospheric  vehicles.  Trajectory  optimization  implies  that  a  vehicle  performance 
measure  is  optimized,  while  ensuring  that  vehicle  structural,  propulsion,  and  thermal  limits 
are  not  violated.  Typical  vehicle  performance  measures  include:  (1)  the  amount  of  fuel 
needed  to  achieve  a  desired  orbit,  (2)  the  time  necessary  to  achieve  a  desired  orbit  or  (3)  the 
vehicle  payload.  Vehicle  structural  limits  include  dynamic  pressure;  propulsion  limits 
include  the  engine  throttle  setting.  Dynamic  pressure  is  an  example  of  a  state  variable 
inequality  constraint;  throttle  setting  is  an  example  of  a  control  variable  inequality 
constraint. 


State-Control  Constraints  w  (x  (r),  u  (r),  /)  £  0  ( \  5) 

A  more  detailed  discussion  of  these  equations  is  given  in  Chapter  2.  In  general,  (1.5)  may 
represent  a  state  variable  inequality  constraint,  a  control  variable  inequality  constraint  or  a 
state-control  variable  inequality  constraint 
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1.3  Trajectory  Optimization 

Several  classes  of  techniques  for  solving  constrained  optimization  problems  have 
been  developed.  These  include  penalty  functions,  steepest  ascent,  and  sequential  gradient 
restoration  algorithms.  In  the  remainder  of  this  section,  algorithms  for  solving  constrained 
optimization  problems  are  briefly  reviewed.  The  review  begins  with  some  remarks  about 
penalty  functions  and  dynamic  programming.  The  first  technique  described  in  detail  is  the 
technique  that  was  developed  by  Bryson  and  Denham  [2,3]  in  the  1960s.  The  second  is 
the  sequential  gradient  restoration  algorithm  (SGRA)  developed  by  Miele  and  his 
colleagues  [4],  The  third  algorithm  is  based  on  a  new  technique  developed  by  Potter  [5,6]. 

1.3.1  Penalty  Functions 

Penalty  function  approaches  represent  an  indirect  method  for  solving  nonlinear 
constrained  optimization  problems  [7].  Typically,  penalty  function  approaches  account  for 
problem  constraints  by  adding  terms  to  the  performance  index  to  penalize  constraint 
violations,  and  then  solve  the  resulting  unconstrained  optimization  problem.  Although 
intuitively  appealing,  considerable  difficulties  can  be  encountered  in  the  implementation  of 
the  penalty  function  approach.  The  application  of  penalty  functions  is  essentially  an 
iterative  process  wherein  hard  constraints  are  guaranteed  to  be  satisfied  only  when  a  free 
parameter  iteratively  approaches  an  indefinitely  large  or  small  value.  As  the  parameter 
approaches  it's  limit,  the  unconstrained  performance  measure  can  become  dominated  by  the 
constraint  penalty  function,  with  the  result  that  the  original  constrained  problem  is 
obscured. 

1.3.2  Bryson's  Algorithm 

Steepest  ascent  techniques  were  first  applied  in  solving  the  aerospace  problems  of  the 
1960s.  Bryson  and  his  colleagues  developed  a  steepest  ascent  algorithm  for  solving 
optimization  problems  when  state  and  control  inequality  constraints  are  present  [2,3]. 
Bryson's  original  problem  formulation  requires  a  Mayer  form  of  performance  measure 


3 


Chapter  1  Introduction 


such  as  that  shown  in  (1.1).  The  steps  in  an  algorithm  that  implements  Bryson's  approach 
are  outlined  below. 

1)  Postulate  a  nominal  control  Uo(0-  (This  is  the  initial  guess  at  the 

solution.) 

2)  Using  u0(r)  and  the  initial  condition  x(r0)  =  Xo,  integrate  the  system 
dynamics  Equation  (1.2)  to  obtain  a  state  trajectory  x(t). 

3)  Simultaneous  with  2)  evaluate  the  constraints  in  (1.5). 

a)  If  a  constraint  boundary  is  met  or  exceeded,  (i.e.,  if  w  £  0  in 
Equation  (1.5))  then  determine  u'0((),  to  maintain  the  state 
trajectory  on  the  constraint  boundary  w  =  0  and  integrate  the 
dynamics  with  this  control  until  the  state  trajectory  leaves  the 
constraint  boundary  under  the  original  nominal  control  uQ(r). 

Upon  leaving  the  boundary,  continue  using  the  nominal  uQ(r). 

( The  quantity  u'0(()  is  determined  from  the  constraint  (1.5). 

There  are  some  minor  differences  in  this  step  between  the 
initial  pass  and  subsequent  passes.  Also,  some  technical 
details  associated  (i)  with  the  augmentation  of  the  constraints 
in  Equation  (1.5)  and  (ii)  with  the  determination  of  both  the 
time  of  departure  from  a  constraint  boundary  and  the  control  to 
be  applied  during  the  process  of  that  departure  have  been 
omitted.  See  [3]  for  details.) 

b)  If  no  constraint  boundary  is  met,  use  the  nominal  control  u0(f). 

4)  At  the  final  time  tf,  evaluate  Equation  (1.4).  If  the  final  time  is  free 
determine  (/via  a  stopping  condition.  The  stopping  condition  is 
defined  in  terms  of  one  of  the  components  of  the  terminal  constraint 
equation  being  satisfied 

5)  Compute  the  values  of  influence  functions  that  determine  how  the 
value  of  the  performance  index  and  violations  of  the  constraints  are 
affected  by  variations  in  the  initial  state  and  the  nominal  control. 

6)  Specify  the  value  P2  that  fixes  a  measure  of  the  energy  of  the  control 
perturbations,  6u((),  that  are  allowed  over  the  current  iteration: 
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P2=(  Su(r)TW(f)8u(0df 

(1.6) 

7)  Using  the  information  generated  above,  an  auxiliary  linear  quadratic 
optimization  problem  is  solved  to  determine  the  appropriate  control 
perturbations  8u(f). 

8)  Check  an  appropriately  defined  convergence  criterion 

a)  If  the  problem  has  converged  then  stop 

b)  Otherwise,  update  the  nominal  control  Uo(f)  by 
u 0(t)  =  u 0(t)  +  8u(r)  and  return  to  2) 

Because  this  approach  is  essentially  a  first  order  technique,  convergence  will  be  slow 
near  the  optimal  solution.  Additionally,  the  technique  is  greatly  complicated  when  the 
trajectory  hits  the  constraint  for  a  period  of  time,  then  comes  off  the  constraint  for  a  period 
of  time  and  then  goes  back  on  the  constraint  at  some  future  time.  Because  of  finite 

computation,  small  errors  will  be  present. 

1.3.3  Sequential  Gradient  Restoration  Algorithm  (SGRA) 

The  sequential  gradient  restoration  algorithm  consists  of  two  distinct  stages:  a 
gradient  stage  and  a  restoration  stage.  The  objective  of  the  gradient  stage  is  to  improve  the 
value  of  the  objective  function  while  disallowing  significant  constraint  violation.  The 
restoration  phase  works  at  satisfying  constraints,  while  preventing  large  changes  in  the 
objective  function.  The  algorithm  alternates  between  the  two  stages  to  converge  to  an 
optimal  solution.  Using  this  technique,  one  would  usually  start  with  a  restoration  stage 
first  since  it  is  rarely  possible  to  guess  a  priori  a  nominal  control  that  satisfies  all  of  the 
constraints. 

The  sequential  gradient  restoration  algorithm  has  been  developed  to  handle  a  slightly 
modified  version  of  the  problem  stated  in  Equations  (1.1-1. 5).  The  technique  requires  that 
the  state  and  control  constraint  in  Equation  (1.3)  be  a  strict  equality.  Secondly,  the  control 
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vector  is  decomposed  into  an  independent  control  vector  and  a  dependent  control  vector. 

The  dependent  control  vector  ensures  that  the  equality  constraint  is  satisfied. 

Consequently,  the  number  of  constraints  must  be  no  greater  then  the  number  of  controls. 

With  these  modifications  to  the  problem  statement,  the  sequential  gradient  technique 

proceeds  as  follows: 

Gradient  Stage 

1)  Determine  a  nominal  control  Uo(f)  that  satisfies  the  constraints  in 
Equations  (1.2- 1.5)  to  within  a  user  specified  accuracy.  Using  this 
information,  a  set  of  linearized  equations  can  be  determined. 

2)  Choose  M-i,  the  Lagrange  multiplier  of  the  terminal  constraint  (1.4),  in 
the  following  manner: 

M-i =  ei  i =  (i7) 

where  b  is  the  dimension  of  the  terminal  manifold  and  ei  is  the  b  x  1 
unit  vector  along  the  i*  direction.  Choose 

14+1=°  (1.8) 

3)  For  each  of  the  choices  of  Hi  in  2)  compute  the  Lagrange  multiplier 
Xj(t)  associated  with  (1.2).  Computation  of  the  Xi  requires  the 

•  backward  integration  of  a  system  of  linear  ordinary  differential 

equations. 

4)  The  actual  value  of  p  is  a  linear  combination  of  the  p;.  Similarly,  the 
actual  solution  for  X(t)  is  a  linear  combination  of  the  Xi(t).  The 
solution  of  this  problem  is  obtained  from  a  system  of  linear  equations. 

5)  Compute  the  variations  in  the  independent  control  (by  minimizing  a 
quadratic  cost  function  chosen  to  improve  performance),  the 
dependent  control  and  the  state  per  unit  stepsize  from  the  nominal 
trajectory  and  the  linearized  system. 

6)  Determine  the  appropriate  stepsize  via  a  one  dimensional  search. 

7)  Compute  the  variations  in  the  control  from  5)  and  6) 
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8)  Update  the  nominal  control  according  to  the  formula 
uo(0  =  u0(r)  +  5u  (r). 

Restoration  Stage 

1)  Assume  a  nominal  control  Uo(f)  that  may  violate  one  or  more  of  the 
constraints  in  Equations  (1.2- 1.5). 

2)  Choose  Hi,  the  Lagrange  multiplier  of  the  terminal  constraint  ( 1 .4)  in 
the  following  manner 

Hi  =  ei  (i.9) 

where  b  is  the  dimension  of  the  terminal  manifold  and  e;  as  described 
in  (1.7). 

3)  For  each  of  the  choices  of  Hi  in  2),  compute  the  Lagrange  multiplier 
Xj(t)  associated  with  (1.2).  Computation  of  the  Xj  requires  the 
backward  integration  of  a  system  of  linear  ordinary  differential 
equations. 

4)  The  actual  value  of  H  is  a  linear  combination  of  the  Hi-  Similarly,  the 
actual  solution  for  X(t)  is  a  linear  combination  of  the  Xi(t).  The 
solution  of  this  problem  requires  the  solution  of  a  system  of  linear 
equations. 

5)  Compute  the  variations  in  the  independent  control,  (by  minimizing  a 
quadratic  cost  function  chosen  to  improve  constraint  satisfaction)  the 
dependent  control  and  the  state  per  unit  stepsize  from  the  nominal 
trajectory  and  the  linearized  system. 

6)  Determine  the  appropriate  stepsize  via  a  one  dimensional  search. 

7)  Compute  the  variations  in  the  control  from  5)  and  6) 

8)  Update  the  nominal  control  according  to  the  formula 
u  o(0  =  uo(0  +  5u(r). 

The  only  significant  difference  between  the  gradient  stage  and  the  restoration  stage  is 
in  Step  1).  The  gradient  stage  requires  that  the  constraints  be  satisfied  to  within  a  user 
specified  degree  of  accuracy,  while  the  restoration  stage  does  not.  Both  stages  of  the 
technique  require  the  determination  of  a  step  size  parameter  in  step  5)  by  solving  a  different 
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quadratic  optimization  problem  for  the  independent  control.  Termination  occurs  when  a 
prespecified  convergence  criterion  is  satisfied. 

The  sequential  gradient  restoration  technique  is  unique  in  several  respects.  Strictly 
speaking,  it  has  been  developed  to  solve  problems  over  a  normalized  time  interval 
0  £  r  £  1.  At  first  glance,  this  may  appear  to  be  a  serious  limitation.  The  ability  to 
handle  free  final  time  problems  could  be  jeopardized.  However,  as  demonstrated  by  Lee 
[8]  this  is  not  a  serious  shortcoming  of  the  methodology.  By  applying  a  suitable 
transformation  to  the  problem,  the  time  interval  of  interest  can  be  made  arbitrary.  SGRA 
methods  only  direcdy  handle  equality  constraints.  In  order  to  handle  inequality  constraints, 
a  transformation  must  be  applied.  Reference  [8]  illustrates  how  this  can  be  accomplished 
as  well.  One  interesting  feature  of  the  sequential  gradient  restoration  algorithm  is  that 
ultimately,  only  a  quadratic  programming  problem  needs  to  be  solved.  Unfortunately,  the 
sequential  gradient  technique  requires  repeated  integration  of  the  Lagrange  multipliers.  The 

full  details  of  this  technique  are  contained  in  [4]. 

1.3.4  Potter's  Algorithm 

The  algorithm  developed  by  Potter  considers  a  slightly  different  problem  formulation. 
In  particular,  the  performance  index  is  a  function  only  of  the  initial  information,  e.g., 

Jnl  =  ^x(t0,fo) 

In  this  thesis,  the  viability  of  using  the  performance  measure  (1.1)  in  Potter's  algorithm 
will  be  investigated.  Secondly,  the  approach  is  inherently  discrete.  The  steps  in  Potter's 
algorithm  are  summarized  below. 

1)  Postulate  a  nominal  control  Uo(r). 

2)  Using  Uq (/)  integrate  the  system  dynamics. 

3)  Simultaneous  with  2)  evaluate  the  constraints  in  (1.5).  If  a  constraint 
is  violated,  save  the  following  information:  the  sample  at  which  the 
violation  occurs,  the  amount  of  the  violation,  the  impact  of  changes  of 
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the  state  on  the  constraint  and  the  impact  of  changes  of  the  control  on 
the  constraint  (details  about  this  step  are  discussed  in  Chapter  2) 

4)  Determine  a  linearized  version  of  the  problem  and  determine  a  set  of 
equality  constraints  in  terms  of  the  state  and  control  perturbations. 

These  equality  constraints  will  serve  to  correct  constraint  violations 
and  are  only  present  during  times  when  the  constraints  were  violated 
in  2). 

5)  Construct  a  discrete-time  quadratic  cost  in  terms  of  perturbations  in  the 
controls. 

6)  Using  the  information  from  2),  3)  and  4)  solve  an  auxiliary  linear 
optimization  problem,  where  the  cost  function  is  a  discrete-time 
quadratic  cost  in  terms  of  the  perturbations  in  the  control. 

7)  Using  the  square  root  sweep  algorithm  [6],  compute  8u(r),  the 
perturbations  in  the  control. 

8)  Check  a  user  specified  convergence  criterion 

a)  If  the  problem  has  converged  then  stop 

b)  otherwise,  update  the  control  and  go  to  2) 

The  most  notable  features  of  Potter's  algorithm  are  in  steps  3)  and  7).  In  step  3)  the 
nominal  state  trajectory  is  permitted  to  exceed  the  problem  constraints.  In  Bryson's 
technique  discussed  earlier,  the  control  is  computed  or  modified  so  that  the  constraints  are 
not  violated.  Step  7)  is  a  new  technique  for  computing  the  perturbations  in  the  control.  It 
is  essentially  a  descendant  of  the  successive  sweep  algorithm  of  optimal  control  theory,  but 
with  a  special  modification  to  handle  intermediate  equality  constraints  in  the  state  and 
control. 

1.3.5  Trajectory  Optimization  Algorithm  Summary 

The  previous  techniques  are  representative  of  the  state-of-the  art  for  solving 
constrained  optimization  problems.  This  thesis  focuses  on  developing  a  sound 
understanding  of  Potter's  technique. 
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The  various  techniques  described  above  share  many  common  features  and  fit  into  a 
basic  solution  framework  or  algorithm.  The  steps  of  this  basic  algorithm  are  outlined 
below. 

1)  Develop  a  nominal  control  program 

2)  Using  the  nominal  control,  compute  the  nominal  state  trajectory  and  a 
linearized  perturbation  model  of  the  system. 

3)  Check  the  terminal  constraints  and  the  inequality  constraints. 

Determine  a  set  of  corrections  that  will  improve  constraint  satisfaction. 

4)  Compute  small  perturbations  to  the  nominal  control  trajectory  to  better 
satisfy  constraints  and  improve  the  performance  measure. 

5)  Update  the  nominal  control  with  the  control  perturbations  and  analyze 
the  constraints  and  performance  of  the  new  nominal  control. 

6)  If  the  new  nominal  control  yields  desirable  constraint  satisfaction  and 
performance  continue  with  Step  7.  Otherwise,  return  to  Step  3)  and 
try  again. 

7)  Determine  whether  the  solution  has  converged.  If  the  the  solution  has 
not  converged  then  continue  at  Step  2);  otherwise  stop. 

The  seven  steps  listed  above  describe  a  basic  algorithm  for  solving  trajectory  optimization 
problems.  Each  of  the  techniques  discussed  in  this  section  addresses  these  steps  in  its  own 
unique  fashion.  The  most  significant  differences  occur  in  Step  4)  when  the  control 
perturbations  are  computed.  Step  4)  is  the  gradient  step  of  the  optimization  problem. 

1.4  Thesis  Organization 

The  emphasis  of  this  thesis  is  on  Potter’s  algorithm  [5,6].  Chapter  2  discusses  the 
problem  to  be  solved  in  greater  detail.  Potter's  algorithm  computes  discrete  control 
perturbations  by  solving  a  discrete-time  linear  quadratic  optimization  problem.  The 
connection  between  the  nonlinear  optimization  problem  and  the  discrete-time  linear 
quadratic  optimization  problem  is  discussed  in  Chapter  2.  In  Chapter  3,  Potter's  solution 
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algorithm  for  the  control  perturbations  is  discussed.  The  solution  algorithm,  the  square 
root  sweep  algorithm,  consists  of  a  sequence  of  backwards  and  forward  recursions  that 
yield  the  appropriate  control  perturbations.  Chapter  4  shows  how  the  technique  can  be 
used  to  satisfy  constraints  and  improve  a  Mayer  form  of  performance  measure.  In  Chapter 
5,  the  square  root  sweep  technique  is  applied  to  the  re-entry  portion  of  flight  for  a  lifting 
glide  vehicle.  The  emphasis  in  Chapter  5  is  the  development  of  trajectories  that  satisfy 
constraints.  Simultaneous  constraint  satisfaction  is  demonstrated  and  the  ability  to  satisfy 
hard  state-control  constraints  and  boundary  conditions  are  demonstrated.  Finally  in 
Chapter  6,  conclusions  and  recommendations  for  future  research  are  discussed. 
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2  Problem  Development 

2.1  Introduction 

In  this  chapter,  the  problem  to  be  solved  is  formulated.  The  problem  to  be  solved 
represents  a  nonlinear  constrained  optimization  problem  with  hard  inequality  constraints. 
The  physical  system  will  be  modeled  using  nonlinear  ordinary  differential  equations. 
Constraints  will  be  of  two  types:  (1)  boundary  conditions  and  (2)  hard  inequality 
constraints  on  the  state  and  control  variables.  Although  not  necessary,  the  initial  time  and 
the  final  time  will  be  fixed. 

Nonlinear  optimization  problems,  like  the  one  developed  in  this  chapter,  are  quite 
difficult  to  solve.  Because  of  the  presence  of  nonlinearities,  iterative  numerical  techniques 
are  usually  used  to  solve  the  nonlinear  optimization  problem.  Most  solution  techniques. 
Potter's  square  root  sweep  algorithm  included,  begin  by  postulating  a  nominal  control  input 
and  then  compute  small  perturbations  to  the  nominal  control  input  A  linear  perturbation 
model  is  used  to  assess  the  impact  of  the  control  perturbations  on  the  nominal  state 
trajectory,  the  hard  inequality  constraints,  the  boundary  conditions  and  the  performance 
measure.  Because  the  problem  is  solved  using  a  digital  computer,  a  constrained 
discrete-time  linear  quadratic  optimization  problem  is  developed.  The  solution  of  the 
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constrained  discrete-time  linear  quadratic  optimization  problem  should  yield  control 
perturbations  that  improve  performance  and  better  satisfy  the  inequality  constraints  and 
boundary  conditions. 

2.2  Problem  Formulation 

Consider  the  system  of  first-order  ordinary  differential  equations  given  by  the  vector 
equation 

x(/)  =  f  (x(t),  u(r),f)  t  e  (2.i) 

where  x(r)  represents  the  /i-dimensional  state  vector,  u(f)  represents  the  m-dimensional 
control  input  and  t  represents  the  independent  variable.  Typically,  the  independent  variable 
will  be  time;  however,  this  is  not  always  necessary  or  desirable  [9].  In  general,  (2.1)  is 
assumed  to  be  nonlinear. 

The  objective  is  to  determine  the  control  input  u(r)  which  optimizes  the  scalar 
performance  measure 


Jnl  =  ViMM  (2.2) 

The  problem  of  optimizing  the  performance  measure  in  (2.2)  represents  a  Mayer  problem  in 
the  calculus  of  variations.  This  problem  can  be  shown  to  be  equivalent  to  the  Bolza  and 
Lagrange  problems  [10].  Earlier  efforts  by  Potter  [1 1]  focused  on  performance  measures 
written  in  terms  of  x(r0)  and  t0. 

In  addition  to  the  dynamic  constraint  in  (2.1),  several  other  types  of  constraints  may 
be  present.  First,  it  may  be  desirable  to  require  that  the  initial  and/or  final  state  lie  on  a 
prescribed  manifold  in  state  space.  This  requirement  can  be  written  mathematically  as: 
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q(4W=o 

where  q  represents  a  r-dimensional  vector  of  initial  constraints  and  m  represents  a 
p -dimensional  vector  of  terminal  constraints.  To  ensure  well-posedness,  it  is  assumed  that 
p  £  n  and  that  r  <,n.  Otherwise,  all  constraints  may  not  be  simultaneously  achievable. 
That  is,  the  constraints  may  conflict  with  one  another. 

It  may  be  necessary  to  restrict  the  state  and  control  to  some  region  of  state  and  control 
space.  Constraints  of  this  form  typically  arise  from  physical  considerations.  In  aerospace 
problems,  these  constraints  can  be  due  to  structural  load  limits  such  as  dynamic  pressure  or 
propulsion  limits  such  as  maximum  allowable  thrust.  These  constraints  are  expressed  as 
inequalities  of  the  form 

»(M')SO  (2.4) 

w  represents  a  ^-dimensional  vector  of  inequality  constraints.  In  general,  it  is  not 
necessary  that  both  the  state  and  control  be  present  in  (2.4). 

In  summary,  the  problem  to  be  solved  can  be  written  as  a  continuous  time 
optimization  problem  with  nonlinear  dynamics  and  hard  inequality  constraints. 

Performance  Index  JNL  =  ^x(r/),t/)  (2.5) 


(2.3a) 

(2.3b) 


System  Dynamics 


Initial  Constraints 


Terminal  Constraints 


x(r)  =  f(x(r),u(r),r)  t  e 


q(x(f0),'o)  =  o 


(2.6) 

(2.7) 

(2.8) 
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State-Control  Constraints  w  (x  (f), u  (r),r)  <,  0 


(2.9) 


A  large  number  of  problems  of  interest  to  the  aerospace  community  fit  into  the  framework 
of  (2.S-2.9) 


2.3  Linearization 

Similar  to  traditional  gradient  optimization  algorithms,  the  solution  method  developed 
in  this  thesis  begins  with  a  nominal  control  history  Uo(f)  that  satisfies  the  system  dynamics 
(2.6).  While  the  system  dynamics  (2.6)  are  satisfied,  boundary  conditions  (2.7-2. 8)  and 
state-control  constraints  (2.9)  may  not  be  satisfied.  To  ameliorate  this  situation,  small 
adjustments  to  the  control  program  are  made.  These  adjustments,  also  called  control 
perturbations,  should  decrease  constraint  violations,  improve  boundary  condition 
satisfaction  and  improve  performance.  To  assess  the  impact  of  the  control  perturbations  on 
the  state  trajectory,  a  linearized  perturbation  model  is  used.  In  this  section,  a  linearized 
perturbation  model  is  developed. 

For  given  values  of  x(rc),  t0  and  the  nominal  control  input  uo(0  the  solution  to  (2.6) 
will  be  denoted  by  x0(f).  If  the  initial  conditions  are  perturbed  by  a  small  amount  to 
x(f0)  +  Ax(r0)  and  the  nominal  control  is  perturbed  by  a  small  amount  to  uo(0  +  Au0(r), 
then  one  would  expect  the  perturbed  solution  of  (2.6)  to  be  Xq(0  +  Axo(r)  where  Ax0(r)  is 
small.  Expanding  as  a  Taylor's  series  about  the  nominal  path  yields 


x  (0  =  f  (xo^Uo^r)  +A  (r)  (x  (f)  -  Xo(f))  +  B  (t)  (u  ( t )  -  u  Jit))  +  •••  (2.10) 


Xo^UoM.f 


'Ml 

Ml  1 

dxi 

dxn 

Ml 

Ml 

dxn  J 

XoM,Uo(t),/ 


(2.11) 
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Xo(r),Uo(r),f 


M. 

a/i  1 

du\ 

dfn_ 

a/« 

_  du\ 

xJ[tl\iJit),t 


(2.12) 


Because  x0(r)  satisfies  the  nonlinear  differential  equation  (2.6)  (i.e. 
dx0(t)/dt  =  f(xo(/),uo(r),0)  then 


=  A  (»)  Ml)  -  Xo(»))  +  B  (l)  Ml)  -  UoM)  +  •  ■ . 


(2.13) 


Truncating  the  higher  order  terms,  an  approximation  to  the  true  differential  equation 
satisfied  by  x(r)  -  Xo(r)  is  obtained: 


8x(r)  =  A(t)6x(r)  +  B(r)6t<r) 


(2.14) 


where  5x(f)  =  x(f)  -  x0(r)  and  8u(f)  =  u(f)  -  uo(0-  This  last  equation  is  a  linear  time 
varying  ordinary  differential  equation  and  is  called  a  linearized  perturbation  equation. 

If  A(f)  and  B(f)  are  piecewise  continuous,  then  the  solution  to  the  linearized 
perturbation  equation  takes  the  form 


5x(f)  =  <b(t,t0)  8x(r0)  +  <D(/,T)B(r)8u(x)dt 


( 

Jt. 


(2.15) 


where  <D(v)  is  the  n  x  n  state  transition  matrix.  The  state  transition  matrix  satisfies  the 
differential  equation 


d$(t,t0) 

dt 


=  A(r)<t(r,r0) 


(2.16) 
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with  initial  condition 


^(*0>*o)  -  Ifl 


(2.17) 


2.4  Discretization 

Problems  of  the  form  described  by  (2.5-2.9)  are  usually  quite  difficult  to  solve.  This 
is  especially  true  for  problems  of  any  physical  significance  and  complexity.  Solutions  to 
problems  of  this  form  usually  rely  upon  the  use  of  numerical  solution  on  a  digital 
computer.  Without  loss  of  generality,  assume  that  t0  =  0. 

Consider  N  points  in  the  interval  0  <,t<tf  not  necessarily  equally  spaced.  These 
points  will  be  denoted  by  r*  where  k  =  0,...,  N.  The  nominal  control  input  uc(i)  will  be 
approximated  as  a  piecewise  constant  function  that  changes  only  at  r*  for  k  =  0,...,  N  -  1. 
For  notational  brevity,  the  nominal  control  input  Uo(0  will  be  written  as  u(jfc).  The  nominal 
state  trajectory  can  then  be  computed  by  numerically  integrating  the  system  dynamics  (2.6). 
The  resulting  values  of  the  state  at  the  times  f*.will  be  written  as  x(k).  The  performance 
measure  becomes 


Jnl=  *(<"),  N) 


(2.18) 


while  the  remaining  constraints  are 


q(x(0),0)  =  0 


(2.19) 


m  {x(N\  N)  =  0 


(2.20) 


w(x(*),  u(*),  k)£  0 


a.2n 
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The  value  of  O (k)  is  found  by  numerically  integrating  the  differential  equation  in  (2.16) 
subject  to  the  initial  condition  (2.17).  The  value  of  G(£)  can  be  computed  by  numerically 
integrating  the  differential  equation 
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=  Ht )G(t,(„)  +  B(|)  G((o,<o)  =  0 

«  (2.27) 


2.5  Discrete  Optimization  Problem 

For  well-behaved  nonlinear  systems,  small  perturbations  in  the  control  variables  and 
the  initial  conditions  will  cause  small  changes  in  the  performance  and  the  constraints.  To 
first-order  these  variations  are  given  by 


&NL  = 


a*p 


[MNh{N\ 


8x(N) 


8m  = 


dm 


[MNh(Nl 


8x(N) 


(2.28) 

(2.29) 


8q  = 


dq 


3^0)lx(0). 


8x(0) 


(2.30) 


Alternatively  if  the  quantities  8Jnl,  8m,  and  8q  are  specified,  (2.28-2.30)  can  be  interpreted 
as  constraints  on  the  allowable  variations  of  6 x(N)  and  8x(0).  Typically,  these  quantities 

are  specified  as  some  fixed  percentage  of  the  current  value  obtained  from  the  solution  of  the 
nonlinear  problem  for  a  given  value  of  x(r0)  and  uo(0  (i.e.  8Jnl  =  cjJnl.  8m  =  -  cmm, 
and  8q  =  -  cflq;  where  a  positive  value  of  cj  is  used  for  a  maximization  problem,  a 
negative  value  of  Cy  is  used  for  a  minimization  problem  and  0  £  cy  £1,  0£cm£l, 
0  £  cq  £1). 


Small  changes  in  the  state  and  control  will  also  affect  the  state-control  constraint 
(2.9).  The  impact  of  these  changes  is  given  by 


8w {k)  = 


dw  | 

53nU(*+ 


i)^)J 


8x(k  +  1 )  -H 


dw 


W*h(lk+l)v(lkl 


Sifk) 


(2.31) 
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When  values  for  8w(£)  are  specified,  the  last  equation  can  be  interpreted  as  a  constraint  on 
the  values  of  8u (k)  and  bx(k  +1)  that  yields  a  predetermined  change  in  the  value  of  the 
state-control  inequality  constraint.  The  value  of  8w  is  chosen  to  improve  inequality 
constraint  satisfaction.  Equation  (2.31)  is  also  a  statement  about  the  causality  of  physical 
systems,  since  the  value  of  8x(£  +  1)  depends  on  past  values  of  the  control  perturbations 
and  not  future  values. 

The  choice  of  the  values  of  cj,  cm,  cq  and  8w(Jt)  are  somewhat  arbitrary  and  are 
highly  problem  dependent.  Given  a  choice  of  the  parameters  cj,  cm ,  cq  and  8w(ik),  the 
resulting  trajectory  may  exhibit  better  or  worse  performance  and  constraint  satisfaction. 
For  example,  if  the  choices  result  in  a  trajectory  that  exhibits  better  constraint  satisfaction 
and  better  performance,  then  the  values  of  Cj,  cm,  cq  and  8w(£)  specified  are  reasonable  and 
should  be  accepted.  On  the  other  hand,  if  the  choices  result  in  a  trajectory  that  exhibits 
worse  constraint  satisfaction  and  worse  performance,  then  the  values  of  cj,  cm,  cq  and 
8w(/r)  specified  are  unreasonable  and  should  be  reduced.  If  the  resulting  trajectory  exhibits 
better  constraint  satisfaction  but  worse  performance,  then  the  appropriate  action  is  not  quite 
obvious-the  appropriate  choice  depends  upon  how  much  the  performance  is  worsened.  If 
the  performance  degradation  is  small,  as  measured  by  a  user  specified  metric,  then  the 
values  specified  should  be  accepted.  Otherwise,  the  value  of  cj  is  too  large  and  should  be 
reduced.  If  the  resulting  trajectory  exhibits  worse  constraint  satisfaction  and  better 
performance,  then  the  values  of  cj ,  cm,  cq  and  8w (k)  should  be  reduced. 

The  current  method  of  handling  inequality  constraints  differs  significantly  from 
previous  methods.  It  should  be  noted  that  the  equality  constraint  given  by  (2.31)  is  only 
active  at  times  r*  when  the  actual  nonlinear  trajectory  violates  a  hard  inequality  constraint. 
As  such,  it  can  be  interpreted  as  an  equality  constraint  on  the  values  of  8u(k)  and  8x(k  +  1) 
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whose  value  8w (k)  is  chosen  so  that  the  constraint  violation  is  eliminated  or,  at  least, 
reduced. 

In  the  technique  developed  by  Bryson  and  Denham  [3],  violations  of  the  hard 
inequality  constraints  are  not  allowed.  In  the  Bryson  and  Denham  technique,when  a 
constraint  boundary  is  reached  the  nominal  control  u0(f)  is  altered  so  that  the  nominal 
trajectory  x0(f)does  not  violate  the  constraint  (2.9).  In  addition,  the  current  technique 
handles  state  inequality  constraints  w(x(f ),/)  just  as  easily  as  control  inequality  constraints 
w(x(f  ),u(r),/)  or  w(u(f  ),r).  Traditional  steepest  descent  techniques,  like  the  one  developed 
by  Bryson  and  Denham[2,3],  require  that  state  inequality  constraints  be  converted  to  a 
control  inequality  constraint  by  differentiating  the  constraint  until  the  constraint  depends 
explicitly  on  the  control.  In  the  process,  each  subsequent  differentiation  becomes  a 
constraint  on  the  original  problem. 

Equations  (2.24,2.28-2.31)  serve  as  the  basis  of  the  problem  solved  by  the  square 
root  sweep  algorithm  developed  in  Chapter  3.  To  simplify  the  notation,  these  equations 
will  be  rewritten  in  the  following  manner.  A  terminal  constraint  equation  will  be  written 
from  (2.28)  and  (2.29)  as 


where  B(A/)  is 


B(N)8x(/V)  +  UN)  =  0 


B(W)s 


d*F 


dm 


,^)lx(iV)J 


(2.32) 


(2.33) 


and  the  value  of  the  constraint  b(N)  is 
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t(N)  =  -  ^NL 


(2.34) 


The  initial  constraint  in  (2.30)  will  be  written  as 


B(0)5x(0)+b(0)  =  0 


(2.35) 


where  B(0)  is 


Halt 


(2.36) 


and  the  value  of  the  constraint  b(0)  is 


b(0)  =  -  6q 


(2.37) 


Finally,  the  state-control  constraint  in  (2.31)  will  be  written  as 


H(*  +  l)8x(*  + 1)  +  q*)5i<ifc  )  +  A (*)  =  0 


(2.38) 


where  H(Jfc  +  1)  and  C(Jfc)  are  given  by 


H(*+l)  = 


(2.39) 


M*h(k  +  !)»<*) 


(2.40) 


and  the  value  of  the  constraint  A(k)  is 


A(jfc)  s  -  5w {k) 


(2.41) 
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The  values  of  the  quantities  in  (2.34,  2.37,  2.41)  are  generally  given  as  a  fixed  percentage 
of  the  current  value  of  the  respective  constraint  violation. 

2.6  Linear  Problem 

Consider  the  discrete-time  linear  system 

8x(*  +  1)  =  <D(*)8x(*)  +  G(*)8u(*)  (2.42) 

with  boundary  conditions 

B(0)8x(0)  +  b(0)  =  0  (2.43) 


B(A08x(A0  +  b(A0  =  0  (2.44) 

and  state-control  constraints  of  the  form 

H(*+l)8x(*+l)  +  C(*)8u(*)  +  A(*)  =  0  (2.45) 

where  8x(fc)  is  an  n-dimensional  vector  and  8u(ifc)  is  an  m-dimensional  vector.  Because  the 
state  transition  matrix  is  obtained  by  integrating  a  linear  differential  equation,  it  is 
nonsingular.  To  ensure  well-posedness,  the  following  assumptions  will  be  made: 

1)  B(0)  is  a  rxn  matrix  with  r£n  where  r  is  the  number  of 
unsatisfied  initial  constraints. 

2)  rank{ B(0))  =  r 

3)  B(N)  is  a  ip  +  1)  x  n  matrix  with  (p  +  1)  £  n  where  p  is 
the  number  of  unsatisfied  terminal  constraints. 

4)  rank(B(N))  =  0>  +  l) 
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5)  H(k  +  1)  is  a  qk  x  n  matrix  and  C (k)  is  a  qkx  m  with 
qk£(n  +  m  )  where  qt  is  the  number  of  constraints 
violated  at  sample  k. 

6)  rank([H(k  +  1)  C (k)])  =  qk 

Assumptions  (1),  (3),  and  (5)  ensure  that  (2.43-2.45)  are  not  overdetermined,  while 
assumptions  (2),  (4),  and  (6)  ensure  that  no  duplicate  constraints  are  present. 

The  objective  is  to  determine  the  sequence  of  control  perturbations  bu(k)  that 
minimizes  the  quadratic  cost  functional 

N- l 

Jlin  =  J  X  8uT(k)R(k)Su(k) 

*=0  (2.46) 

where  the  matrix  R(k)  is  an  m  x  m  matrix  and  R(A)  >  0.  The  quadratic  performance  index 
shown  in  (2.46)  has  been  chosen  for  several  reasons.  The  cost  function  shown  in  (2.46)  is 
mathematically  tractable  and  it  is  desirable  to  limit  the  size  of  the  control  perturbations 
8u (k).  By  keeping  5u(Jt)  small,  it  is  hoped  that  the  linearity  of  the  linear  perturbation  model 
will  not  be  exceeded,  while  constraint  satisfaction  and  the  nonlinear  performance  (2.2)  are 
improved. 

The  necessary  conditions  for  optimality  for  this  constrained  discrete-time  linear 
quadratic  problem  are  found  by  augmenting  the  constraints  given  in  (2.42-2.45)  to  the 
performance  index  given  in  (2.46).  The  constraints  are  adjoined  to  (2.46)  by  using  a  set  of 
Lagrange  multipliers.  The  dynamic  constraint  (2.42)  is  adjoined  by  using  the 
n-dimensional  vector  X ,T(k  +  1),  the  state-control  constraint  (2.45)  is  adjoined  by  using  the 
q*-dimensional  vector  \iT(k  +  1),  the  terminal  constraint  (2.44)  is  adjoined  by  using  the 
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(p  +  l)-dimensional  vector  vT(A0  and  the  initial  constraint  (2.43)  is  adjoined  by  using  the 
r-dimensional  vector  vT(0).  The  augmented  performance  index  Jun  becomes 


Jlin  =  i  V(*)R(*)5i<*)  +>-T(*  +  1  i-  8x<*  +1 )  +  <Hk) 8*<*)  +  G(*)Si<*)) 

L  A  * 


\iT{k  +  1)(h(*  +  l)8x(*  +  1)  +  C(*)8t<*)  +  A(fc))j 


vT(0)(b(0)8x(0)  +  b(0))  +  vV)(B(^)5x(N)  +  b(W)) 


(2.47) 


Using  the  calculus  of  variations,  the  necessary  conditions  for  optimality  can  be  derived.  A 
derivation  of  the  necessary  conditions  is  given  in  Appendix  2A  at  the  end  of  this  chapter. 
The  results  arc  summarized  below: 


5v(k)  =  -  +  1)  +  C ^(kUk  +  1)] 


(2.48) 


M*)  =  O  (*)M*  +  1)  +  H T{k)p(k) 


(2.49) 


X(0)  =  -Bt(OMO) 


(2.50) 


UN)  =  B J{NXN)  +  H  t(NUN) 


(2.51) 


Equations  (2.48-2.51)  along  with  (2.42-2.45)  represent  the  necessary  conditions  for 
optimality.  These  equations  do  not  differ  significantly  from  the  unconstrained  equations. 
These  equations  revert  to  the  unconstrained  equations  when  H  (k  +  1)  =  0  and 
C (k  +  1)  =  0.  To  maintain  the  spirit  of  the  classical  results,  these  equations  will  be 
written  in  the  following  manner 
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+  1)  +  CT(*)p(*  +  1)] 

(2.52) 

X(iV)=  B  t(N)v{N) 

(2.53) 

(2.54) 

X(k)  =  0T(k))Xik  +  1) 

(2.55) 

X(0)  =  X(0)  =  -  Bt(0)v(0) 

(2.56) 

A 

where  \(k)  replaces  \(k)  in  (2.47-2.51).  Equations  (2.52-2.56)  are  consistent  with  (2.48- 
2.51).  Unfortunately,  solving  these  equations  is  quite  difficult.  In  the  next  chapter,  a  new 
method  known,  as  the  square  root  sweep  algorithm,  for  solving  these  equations  is 
presented.  Incorporating  the  square  root  sweep  method  into  an  iterative  solution  to  the 
nonlinear  problem  yields  an  algorithm  that  differs  significantly  from  previous  algorithms. 
For  example,  unlike  other  approaches  it  is  not  necessary  to  compute  an  initial  nominal 
control  that  will  not  violate  the  hard  inequality  constraints.  This  is  a  specific  requirement  in 
other  techniques  [3]. 

The  necessary  conditions  for  optimality  represent  a  two  point  boundary  value 
problem.  In  fact,  the  necessary  conditions  constitute  a  non-square  descriptor  system. 
Descriptor  system  theory  has  been  widely  discussed  in  the  technical  literature  [12]. 

2.7  Summary 

In  this  chapter,  a  continuous  time  nonlinear  optimization  problem  with  hard  inequality 
constraints  has  been  discussed.  A  large  number  of  practical  problems  can  be  posed  in  this 
framework.  Because  of  the  complexity  of  such  problems,  a  numerical  solution  of  the 
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underlying  problem  will  be  pursued.  Small  corrections  to  a  nominal  control  trajectory  will 
be  computed  by  solving  a  discrete-time  linear  quadratic  optimization  problem  with  hard 
equality  constraints.  The  connection  between  the  continuous-time  nonlinear  optimization 
problem  and  the  discrete-time  linear  quadratic  optimization  problem  has  been  established  in 
this  chapter. 

The  necessary  conditions  for  the  discrete-time  linear  quadratic  optimization  problem 
have  been  derived.  Unfortunately,  the  necessary  conditions  do  not  provide  a 
straightforward  solution  to  the  discrete-time  linear  quadratic  optimization  problem.  A 
solution  method  will  be  developed  in  the  next  chapter. 

Given  the  continuous-time  optimization  problem  with  nonlinear  dynamics  and  hard 
inequality  constraints,  a  discrete-time  linear  quadratic  optimization  problem  with  hard 
equality  constraints  has  been  developed.  The  steps  that  integrate  the  linear  problem  into  an 
iterative  algorithm  for  solving  the  nonlinear  optimization  problem  are  summarized  below 

Step  1)  Initial  Control  History  Postulate  a  piecewise 
constant  nominal  control  input  u(k)  that  changes  only  at  r* 
for  k  =  0,  A  -  1.  If  some  of  the  initial  states  are  free,  they 
must  be  specified  to  ensure  a  complete  set  of  initial 
conditions. 

Step  2)  Forward  Integration  Integrate  (2.6,2.16,2.27) 
forward  in  time  to  establish  x(Jfc),  O(jfc),  G(Jfc)-the  nominal 
state  trajectory  and  the  linearized  perturbation  model 
dynamics. 
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Step  3)  Constraint  Evaluation  Evaluate  the  terminal 
constraints  (2.20)  and  the  inequality  constraints  (2.22). 

Step  4)  Linear  Problem  Boundary  Condition  Specification 
Compute  (2.33)  and  (2.36).  Specify  the  values  of  (2.34) 
and  (2.37)  so  that  the  boundary  conditions  are  better 
satisfied  and  the  performance  improved. 


Step  5)  Linear  Problem  Inequality  Constraint  Si 
At  samples  where  the  inequality  constraints  in  (2.22)  are 
violated  compute  (2.39)  and  (2.40).  Specify  the  values  of 
(2.41)  so  that  the  inequality  constraints  arc  better  satisfied. 


At  this  point,  all  the  data  required  to  pose  the  discrete-time  linear  quadratic  optimization 
problem  is  available.  In  the  next  chapter  an  algorithm  for  solving  this  problem  is 
developed. 
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Appendix  2A 

The  augmented  performance  index  7/j*  is  given  by 


N-  1 

/&.=  2  |iSuT(fc)R(*)5i<*)  +  XT(*  +  +  1)  +  <t<Jt)Sx(*)+G(*)S^*}] 

k= o 

+  pT(*  +  i{h(*  +  1)5  x(*  +  1)  +  q*)6u(*)  +  A(jfc)]} 

+  vT(o{b(0)5x(0)  +  h(0)]+vV(B{N)5x(N)  +  fa(iV)] 


Define  the  sequence  T(k)  in  the  following  manner 


J  un=  X  {lW-X,fy  +  l)5x(ib  +  1)  +  pT(*  +  l)H(jfc  +  \Mk  +  1)) 
*=  o 

+  vT(0fB(0)5x(0)  +  b(0)]  +  vfyf  B(tf)5x(N)  +  b(/V)] 


hence  (2A.1)  becomes 


N-l 

Jiin=  X  (n^)-XT()k+  l)5x(iS:+  1KH>+  1W+  1)} 

*=  0 

+  vt(o{b(0)5x(0)  +  b(0)]  +  vt(n!b(N)5x(/V)  +  b(N)] 


(2A.3) 


Rearranging  (2A.3),  by  using  summation-by-parts, 

N-l 
k  =  1 

-  \T(N)8x(N)  +  pT(N)H(N)8x(N)  +  vt(0^B(0)8x(0)  +  b(0)]  +  vt(n{b(N)5x(W)  +  b (N)] 

(2A.4) 


The  extremum  of  (2A.4)  is  found  by  computing  the  first  variation  of  Computing  the 
first  variation  of  /&,  and  collecting  like  terms  yields  (2A.5) 
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8//m  = 


N-  1 


vt(0)B(0)  +  s(Sx(0))  +  -^ls(5i<0)) 

35x(0)J  35u(0) 


+  S  -  xT(*) +  HT(*)H(*)|s(Sx(*))  4-?n*is(Su(*))l 

*  =  1  pSx(/t)  J  85u(ik) 

+  [vT(iV)B{N)  +  \iT{N)HiN)  -  xV)]6(5x{N)) 


The  partial  derivatives  in  (2A.5)  are  given  by 

38x(it) 


and 


^  =  8uT(*)R (*)  +*-T(*  +  W)  +  HT(*  +  1W*) 

dou (k) 


Equating  (2A.5)  to  zero  yields  the  necessary  conditions  for  optimality. 

X(k)  =  <t>T{k)>ik+l)  +  HT{kMk) 


l(N)  =  BJ{N)v{N)  +  H T{N)p{N) 

8u (*)  =  -  +  1)  +  C ?(lcMk  +  1)] 


and  since  H^(0)p(0)  =  0,  therefore 


m=-BTmo) 


(2A.5) 

(2A.6) 

(2A.7) 

(2A.8) 

(2A.9) 

(2A.10) 

(2A.11) 
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3.1  Introduction 

The  square  root  sweep  algorithm  first  appeared  in  [5,6].  Potter  originally  developed 
the  algorithm  as  a  method  for  solving  combined  trajectory  and  configuration  optimization 
problems  [11].  The  objective  of  the  present  chapter  is  to  clarify  the  square  root  sweep 
method  and  to  extend  the  square  root  sweep  algorithm  to  the  Mayer  form  of  performance 
index. 

Traditional  sweep  methods  are  used  to  motivate  the  development  of  the  square  root 
sweep  approach  to  solving  the  constrained  discrete-time  linear  quadratic  optimization 
problem  formulated  in  (2.42-2.46)  of  the  previous  chapter.  The  square  root  sweep 
algorithm  will  be  used  to  solve  the  constrained  discrete-time  linear  quadratic  optimization 
problem  that  was  formulated  in  the  previous  chapter.  The  square  root  sweep  algorithm 
consists  of  a  set  of  backward  recursions  for  a  set  of  square  root  sweep  parameters. 
Because  of  the  presence  of  hard  constraints,  the  square  root  of  the  sweep  matrix  is  used 
rather  than  the  traditional  sweep  matrix.  Constraints  are  accommodated  through  the  use  of 
a  scale  factor  matrix.  A  zero  scale  factor  indicates  that  a  hard  constraint  is  present,  while  a 
nonzero  scale  factor  indicates  the  absence  of  a  hard  constraint.  After  deriving  the  square 
root  sweep  algorithm,  several  alternative  expressions  for  the  control  perturbations  are 
derived.  These  alternative  control  formulas  require  the  introduction  of  a  new  variable, 
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referred  to  here  as  the  v-costate.  The  chapter  concludes  with  an  analytical  example  and  a 
summary  of  the  square  root  sweep  algorithm. 

3.2  Unconstrained  Discrete-Time  Linear  Quadratic  Optimal  Control 

For  the  purpose  of  this  discussion,  consider  the  following  linear  quadratic 
optimization  problem: 


min  ^5xT(N)S(A^)6x(N)  +  £  {5xT(*)Q(*)5x(fc)  +  8uT(*)R(*)8t<fc)) 


The  only  constraint  present  is  the  syster  equation 


8x  (it  +  1)  =  <D(jt)  8x(it)  +  G(k)  8i^fc)  8x  (0)  =  8x0 


Without  loss  of  generality,  the  following  assumptions  will  be  made: 


1) S(N)  =  St(N)^0 

2)  Q(A0  =  Qt(A0  ^  0 

3)  R  (N)  =  Rt(A0  >  0 


With  these  assumptions,  the  following  conditions  ensure  an  optimal  solution 


p(it)  =  Q(l:)8x(^a>T0t)p(it+l) 


8^)  =  -R-Hifc)GT(it)p(it+l) 


p(/V)  =  S{N)5x(N) 


3.2.1  Sweep  Matrix  Solution 

To  obtain  a  solution  to  (3. 2-3. 5),  assume  that  p(it)  and  8x(£)  are  related  in  the 
following  manner 
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p(*)  =  S(*)8x(*)  kZN 


where  S(k)  is  an  n  x  n  matrix  called  the  sweep  matrix.  The  basis  for  this  assumption  is  the 
boundary  condition  shown  in  (3.5).  Using  this  relationship,  the  following  recursion  for 
S(Jk)  can  be  derived  [12]. 

m = to + g<*)k(*))ts(* + 1 im + mm) + kmtr<*)k(*) + m  (37) 


where 


(3.8) 

All  of  the  quantities  in  (3.7)  and  (3.8)  are  known,  therefore  the  values  of  K (k)  and  hence 
S(k)  can  be  determined.  With  these  quantities  known,  the  optimal  control  8u(fc)  can  be 
computed.  The  resulting  control  8u(Jfc)  is  a  state  feedback  control. 

3.2.2  Dynamic  Programming  Solution 

An  alternative  method  of  solving  (3.2-3.5)  can  be  found  by  using  dynamic 
programming  and  the  principle  of  optimality.  Denote  the  cost-to-completion  from  the 
current  sample  as  J[k).  At  the  final  sample,  no  control  is  possible  so  the  optimal  value  of 


the  cost-to-completion  is  simply: 


J*{N)  =  l8xT(W)S(W)8x(W) 


where  7* (AO  is  the  optimal  cost-to-completion.  Now  consider  the  cost-to-completion  from 
k  =  N-  1 

J[N -  1)  =  J*{N)  +  (8xfy  -  1)Q(N  -  1  )8x(N -  1)  +  8uT(N -  1 W -  l)8t4N  -  l)j  (3>  10) 


The  optimal  cost-to-completion  J*(N  -  1)  is  given  by 
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J*(N  -  1 )  =  min  [/*(#)  +  8x  V  -  1 W  -  1  )8 x(N  -  1 ) 

Su(iV-  1) 

+  Sufy  -  1)R(N-  l)8u (N  -  1)} 


(3.11) 


First,  substitute  the  expression  for  8x(A/)  from  (3.2)  (i.e.  let  it  =  N  -  1  in  (3.2))  into 
(3.9)  and  (3.9)  into  (3.1 1).  Then  by  differentiating  (3.1 1)  with  respect  to  8u(N  -  1)  and 
setting  the  result  to  zero  yields  the  following  solution  for  the  optimal  control  at  N  -  1 

Si iN  -  1)  =  -  (R(A/  -  1)  +  Gt{N  -  1)S(N  )G{N  -  l))_1GT(iV  -  1)S(N  )G> {N  -  1)8 x(W  -  1) 

(3.12) 

The  coefficient  of  8 x(N  -  1)  is  recognized  as  being  K (N  -  1)  from  (3.8).  Therefore,  the 
optimal  control  can  be  written  compactly  as 


8u(N-  1)=  K(N -  l)8x(A/-  1) 


(3.13) 


Substituting  (3.13)  into  (3.1 1),  the  optimal  cost-to-completion  becomes 

J*{N  -  1)  =  ^  8xV  -  iftw  -  1)  +  G (N-  1)K {N  -  \))TS(Ni<t>{N  -  1)  +  G {N  -  1)K (N  -  1)) 


+  K(N -  ifSHN -  1)K {N -\)  +  Q(N-  l)]Sx(7V -  1) 


(3.14) 


The  quantity  enclosed  in  [•]  is  recognized  as  S (N  -  1),  the  sweep  matrix  at  k  =  N  -  1. 
In  this  context,  the  sweep  matrix  is  usually  called  the  Riccati  matrix.  Equations  (3 .7-3. 8) 
constitute  a  discrete-time  version  of  the  continuous-time  nonlinear  Riccati  matrix  differential 
equation.  Using  the  principle  of  mathematical  induction,  this  result  can  be  shown  to  hold 
for  all  k<>N.  That  is 


/*(*)  =  ±$xT(Jfc)S{*)8x(*)  k£N 


(3.15) 
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Two  observations  can  be  made  regarding  the  sweep  matrix  S (k).  First  from  (3.6), 
the  sweep  matrix  is  the  derivative  of  the  costate  p(/fc)  with  respect  to  the  state  perturbation 
8x(k).  Second  from  (3.15),  the  sweep  matrix  gives  the  cost-to-completion  at  sample  £  as  a 
function  of  the  state  perturbation  8x(k).  The  second  observation  will  be  quite  useful  in 
deriving  the  square  root  sweep  algorithm  later  in  this  chapter. 

3.2.3  Square  Root  Solution 

To  enhance  numerical  stability,  it  is  often  desirable  to  propagate  the  square  root  of  the 
Riccati  matrix,  denoted  by  VS (k),  rather  than  the  Riccati  matrix  S (k)  itself  [13].  Let  the 
following  square  root  matrices  be  defined 


S{k)=fS(k)TfS(kj 


(3.16) 


R(*  -  1)  =  VR(*-1)TVR(*-1) 


(3.17) 


(3l8) 

Each  of  the  matrices  on  the  left  hand  side  of  the  above  equations  is  at  least  positive 
semidefinite  and  hence  the  existence  of  a  square  root  is  ensured  [15,16].  Given  these 
definitions,  the  cost-to-completion  at  k  -  1  can  be  written  in  the  following  fashion 


fs(kjT'{Wi  0 

o  Vr(*-i)tYr(*-i) 
0  0 


0 

0 


YQ(*-l)TYQ*-l) 


8x(*) 
*1<*-1) 
5  x(k-  1) 


(3. 


Equation  (3.19)  is  just  a  square  root  version  of  (3.10)  at  k  -  1.  To  simplify  notation,  the 
vector  z(k)  will  be  defined  as  follows 
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0 

_  0  1 

8 x(k) 

0 

YR(*-1)  0 

8i ik-  1) 

0 

0 

VQ(*-l)J 

8x(it-  1) 

Hence  the  cost-to-completion  J(k  -  1 )  is  given  by 


(3.20) 


(3.21) 


Substituting  for  5 x(k)  in  terms  of  5 x(k-  1)  and  5 u(k-  1)  in  (3.20)  yields  the  following 
expression  for  x(k) 


1S(k)Q(k-  1) 

iS(k)G(k-l) 

i 

s. 

1 

- ‘ 

0 

VR(jt  - 1) 

[  1<Xk-T) 

0 

.  5u(k-l)  _ 

Once  again  to  simplify  the  notation,  define  the  matrix  W (k)  as 


iS(l 1) 
0 

YQ [k  - 1) 


Ys(*Tg<*-i) 

YR(*-1) 

0 


(3.22) 


(3.23) 


The  matrix  W (k)  in  (3.23)  can  be  interpreted  as  a  square  root  matrix  at  k  -  1. 
Unfortunately,  W (k)  possesses  several  undesirable  features.  First  and  foremost,  the 
dimension  of  the  W(ifc)  is  (2n  +  m)  x  (n  +  m)  instead  of  n  x  n.  Second,  it  is  not 
readily  apparent  from  (3.19)  how  to  compute  the  value  of  8u0t-  1)  so  that  the 
cost-to-completion  is  minimized. 

Written  in  terms  of  (3.22)  and  (3.23),  the  problem  to  be  solved  can  be  stated  in  the 
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J*(k  -  1)  =  min  y 

8u(ik  -  1) 


8x(Jfc-  1) 
_8u(*-l) 


T 

W(k)V\k 


8x(fc  - 1 ) 
8u (it  -  1) 


(3.24) 


Similar  to  the  optimal  estimation  problem,  the  problems  listed  above  can  be  treated  by 
premultiplying  (3.22)  by  an  appropriate  matrix  transformation.  In  order  to  preserve  the 
value  of  the  cost-to-completion,  it  is  necessary  that  the  transformation  be  orthogonal.  The 
usefulness  of  an  orthogonal  transformation  is  illustrated  by  the  following  equation 


1_ 

2 


8x(i t  - 1) 
i  8u(i k-  1) 


L 


Sx(Jk  -  1)1| 


1 

2 


8  x(Jfc  - 1) 
8i^Jt- 1) 


W \k)Y{k 


8x(it  - 1) 

L8^-1)J 


(3.25) 


where  the  quantity  W (k)  is  given  by 


vk*)=WM*)  =  t(*) 


Ys(*Jg(*~i) 
0  YR {k  -  1) 

vq*-i)  o 


(3.26) 


Secondly,  the  orthogonal  transformation  can  be  constructed  such  that  the  last  n  rows  of 
W(Jfc)  are  zero  and  the  first  n  rows  are  decoupled  from  8u(ifc-  1).  The  resulting 
expression  for  W (k)  has  the  form 


"<*)  = 


YS(Jk-l)  0 

K(*>D(*-1)  Vgt(*  -  1)S(*)G(*  -  1)  +  R(*  -  1) 
0  0 


(3.27) 
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where  the  right  hand  side  of  (3.27)  is  from  [12].  The  control  perturbation  8u(ifc  -  1)  is 
associated  only  with  the  middle  m  rows  of  the  right  hand  side  of  (3.27).  By  choosing 
8u(k  -  1)  in  the  following  manner, 


-  l 


8i<*-l)  =  -VGT(*  -  l)S{k)G(k-  1)  +  R(*-1)  -  l)8x(*  -  1)  (3  28) 


8u (k  -  1)  can  be  eliminated  from  the  cost-to-completion  expression  (3.25).  The  quantity 
K(Jfc)  is  given  by 


This  expression,  equation  (3.28),  for  8u (k  -  1)  is  consistent  with  the  earlier  result  (3.12). 
With  this  choice  of  8u(/fc-l)  the  only  rows  of  W  (k)  that  contribute  to  the 
cost-to-completion  are  the  first  n  rows.  By  choosing  8u (k  -  1)  in  accordance  with  (3.28), 
the  dependence  of  the  cost-to-completion  on  8u(£-  1)  has  been  eliminated-thus 
minimizing  the  cost-to-completion.  The  resulting  expression  for  the  cost-to-completion 
depends  only  on  the  first  n  rows  of  W(Jfc)  and  the  state  perturbations  8x(Jfc  -  1). 
Therefore,  these  rows  are  identified  as  the  appropriate  update  for  the  square  root  of  the 
Riccati  matrix  which  is  shown  explicitly  as  the  upper  left  hand  nxn  partition  of  the  matrix 
W(Jt)  =  T(k)W (Ic)  in  (3.27).  By  continuing  this  process,  the  value  of  VS(ifc)  can  be 
determined  for  all  k  <  N.  The  value  of  VS(N)can  be  determined  from  the  data  available  in 
(3.1). 


The  problem  considered  thus  far  in  this  chapter  illustrates  the  general  character  of  the 
sweep  method.  In  addition  to  a  set  of  direct  recursions  for  the  Riccati  matrix,  a  recursion 
for  the  square  root  of  the  Riccati  matrix  has  been  developed.  As  will  be  seen  in  the  next 
section,  the  linear  optimization  problem  posed  in  the  previous  chapter  possesses  several 
features  that  make  its  solution  considerably  more  difficult. 
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3.3  Square  Root  Sweep  Method 

Recall  the  constrained  discrete-time  linear  quadratic  optimization  problem  formulated 
in  the  previous  chapter.  Specifically,  the  objective  is  to  minimize  Jiin  shown  below 

1*-  i 

£  5uT(*)R(*)8u(*) 

*=o  (3.30) 


The  constraints  imposed  on  the  discrete-time  linear  quadratic  optimization  problem  consist 
of  the  following: 


5x(k  +  1)  =  <D(£)8x():)  +  G(k)Sv(k) 
B(0)8x(0)  +  b(0)  =  0 
B(A08x(AT)  +  b(A0  =  0 
H {k  +  l)8x(*  +  1)  +  q*)8u(*)  +  A(*)  =  0 


(3.31) 

(3.32) 

(3.33) 

(3.34) 


In  addition  to  (3.31-3.34),  the  necessary  conditions  for  optimality  for  this  problem  were 
derived  in  Appendix  2A  of  Chapter  2. 

3.3.1  (^dratic  Programming 

One  approach  to  solving  the  constrained  discrete-time  linear  quadratic  optimization 
problem  relies  upon  quadratic  programming  [14].  For  example,  the  transition  between 
k  =  N  -  1  and  k  =  N  can  be  viewed  as  the  following  optimization  problem. 


min  ^  {8ut(N  -  1)R(N  -  l)8u(N  -  1)} 


(3.35) 


subject  to  the  following  constraints 

8x (N)  =  Q(N  -  1)8 x(/V-  1)+G (N  -  l)8i +N  -  1) 


(3.36) 
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B(N)8  x(N)  +  b(AO  =  0 


(3.37) 


H(iV)8x(W)  +  C(N  -  l)8i<N  -  1)  +  A(N  -  1)  =  0 


(3.38  ) 


The  dependence  of  the  constraints  (3.37-3.38)  on  8x(N)  can  be  eliminated  by  substituting 
(3.36)  into  (3.37-3.38).  Making  this  substitution,  the  constraints  become 


H (N)Q{N  -  1)  H(N)G(N  -  1 )+  C(N  -  1) 

8x(W-  1)' 

+ 

A  (N-  1) 

B(N)p(N  -  1)  B(N)G(N-1) 

8u(N-  1) 

.  m  . 

Equations  (3.35,3.39)  constitute  a  quadratic  programming  problem  with  linear  equality 
constraints.  Solution  of  quadratic  programming  problems  is  discussed  in  [14,15].  In  [15], 
one  approach  to  solving  the  problem  uses  the  QR  algorithm;  a  second  approach 
incorporates  the  constraint  (3.39)  into  the  cost  function  through  a  weighting  term  and  then 
solves  an  unconstrained  optimization  problem.  Continuing  in  this  fashion,  a  solution  to  the 
constrained  discrete-time  linear  quadratic  optimization  problem  can  be  determined. 

Potter's  method  will  incorporate  the  linear  equality  constraint  into  the  cost  function  in 
a  similar  manner.  What  makes  Potter's  technique  unique  is  the  ability  of  the  algorithm  to 
propagate  information  about  future  constraints  to  previous  samples. 

3.3.2  Square  Root  Sweep  Algorithm  Theory 

In  this  section,  a  square  root  sweep  algorithm  is  derived  to  solve  this  problem. 
Because  of  the  presence  of  the  boundary  conditions  and  the  hard  state-control  equality 
constraints  (3.34),  the  current  problem  is  significantly  more  difficult  than  the  problem 
considered  in  the  previous  section.  The  terminal  constraint  in  (3.33)  can  be  handled  by 
simple  extensions  to  the  classical  sweep  method  [12].  On  the  other  hand,  the  hard 
intermediate  equality  constraints  on  8x(fc  +  1)  and  8u(ifc)  are  not  as  easily  handled.  The 
hard  equality  constraints  can  in  theory  be  accommodated  by  including  a  quadratic  term  of 
the  form 
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[H(*  +  l)8x(*  +  1)  +  q*  Mk  )  +  A(*)]  T[H{jk  +  1)5 x(*  +  1)  +  qjt  )5i<* )  +  A(>t)] 

e  (3.40) 

in  the  quadratic  cost  function  (3.30)  and  then  solving  the  resulting  unconstrained 
discrete-time  linear  quadratic  optimization  problem  for  various  values  of  e  such  that  e-»0. 
Unfortunately,  this  technique  will  not  in  general  ensure  exact  satisfaction  of  the 
intermediate  equality  constraint  since  only  when  e  =  0  is  exact  satisfaction  assured.  In  the 
square  root  sweep  method  derived  in  the  next  section,  scale  factors  for  the  square  root 
sweep  matrix  rows  will  ensure  constraint  satisfaction. 

The  algorithm  derived  in  this  section  consists  of  a  set  of  backward  recursions  for  a  set 
of  square  root  parameters  at  each  sample.  To  facilitate  the  solution  of  the  current  problem, 
a  generalized  quadratic  form  is  assumed  for  the  cost-to-completion.  The  square  root  sweep 
parameters  comprise  the  quadratic  form.  The  square  root  sweep  parameters  at  sample  k 
will  be  denoted  by  s(k),  v(i t),  W(i k)  and  D(k).  The  parameter  s(k)  is  a  nonnegative  scalar, 
v(Jfc)  is  an  /i-dimensional  vector,  W(Jt)  is  the  n  x  n  square  root  sweep  matrix,  and  D(k)  is 
the  n  x  n  scale  factor  matrix. 

The  scale  factor  matrix  D(jfc)  is  a  diagonal  nonnegative  matrix.  The  i-th  diagonal 
element  of  D(fc)  is  associated  with  i-th  row  of  W  (k)  denoted  by  w,-.  If  the  i-th  diagonal 
element  of  D(ifc)  is  zero,  then  the  corresponding  row  of  W  (k)  will  be  called  a  constraint 
row.  The  constraint  rows  of  W (k),  the  square  root  sweep  matrix,  are  required  to  be 
linearly  independent.  Otherwise  duplicate  constraints  with  conflicting  values  may  be 
present.  The  elements  of  v(k)  associated  with  a  zero  scale  factor  can  be  interpreted  as  the 
value  of  the  constraint. 

Define  the  n-dimensional  vector  z(k)  in  the  following  manner 

#)a\M*)8x(*)  +  v(*)  (3.41 ) 
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where  W (k)  and  \(k)  are  the  square  root  sweep  parameters  and  8x(k)  is  the  value  of  the 
state  perturbation.  In  order  to  satisfy  future  constraints  and  the  final  boundary  conditions, 
the  value  of  8x(k)  may  be  restricted.  In  particular,  if  dt  =  0  and  hence  w,T  is  a  constraint 
row,  then  the  value  of  8x(k)  must  satisfy  the  following  equation: 


w?8x(k)  +  vt(k)  =  0 


(3.42) 


The  cost-to-completion  J(k)  is  the  control  cost  to  start  from  sample  k  and  satisfy  future 
state-control  constraints  and  the  terminal  boundary  conditions.  In  terms  of  the  sweep 
parameters,  the  cost-to-completion  is  given  by: 


J[k)  =  d  s{k)  +  X  rt^ddk) 


di(k)*  o 


(3.43) 


where  the  current  value  of  8x(k)  must  satisfy  any  hard  constraints  of  the  form  (3.42) 
present  in  the  problem.  Equation  (3.43)  constitutes  an  assumed  form  for  the 
cost-to-completion  for  the  problem  considered  in  this  section.  Unlike  the  previous  section, 
(3.43)  represents  a  general  quadratic  form  is  assumed  for  the  cost-to-completion. 

Intuitively,  the  cost-to-completion  could  be  written  as: 


4k)  =  i(s(k)  +  zT(kpr\k)4k)) 


(3.44) 


Written  in  this  manner,  the  cost-to-completion  resembles  the  quadratic  penalty  function 
approach  discussed  earlier  in  this  section,  since  the  cost  of  not  satisfying  constraints  is 
large. 

3.3.3  Square  Root  Sweep  Parameter  Boundary  Conditions 

By  developing  a  set  of  backward  recursive  relationships  for  W(k),  D(k)  v(k)  and  s(k) 
information  about  future  state-control  constraints  and  the  terminal  boundary  conditions  can 
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be  communicated  to  earlier  samples.  For  example,  to  ensure  that  the  final  boundary 
conditions  are  satisfied,  the  values  of  W^,  D (N),  v(iV),  and  s(N)  should  be  chosen  in  the 
following  fashion: 


W{N)  = 


D (N)  = 


W 
0 


0  0 

0  I 


m 

0 


v(N)  = 
s(A0  =  O 


(3.45a) 

(3.45b) 

(3.45c) 

(3.45d) 


The  upper  partition  of  W(N)  in  (3.45a)  accounts  for  the  constraints  at  the  final  sample. 
This  is  readily  apparent  from  (3.45b),  since  the  upper  left  partition  of  D(N)  consists 
entirely  of  zero  elements  indicating  that  the  corresponding  rows  of  W(A0.are  indeed 
constraint  rows.  The  remainder  of  the  rows  of  the  n  x  n  matrix  W(A0  are  chosen  to  be 
zero  so  that  the  resulting  value  of  the  cost-to-completion  is  zero.  Because  these  rows  of 
W(A0  are  not  constraint  rows,  the  corresponding  diagonal  elements  of  D(/V)  are  nonzero. 
By  convention  these  nonzero  scale  factors  are  chosen  to  be  unity.  The  identity  matrix  in  the 
lower  right  hand  partition  of  D(N)  in  (3.45b)  indicates  this  choice.  The  value  of  the 
constraint  is  contained  in  the  upper  partition  of  v(N)  in  (3.45c).  Since  the 
cost-to-completion  at  the  final  sample  is  zero,  it  is  necessary  that  the  scalar  quantity  s(N)  be 
chosen  to  be  zero  as  shown  in  (3.45d). 

3.3.4  Derivation  of  the  Square  Root  Sweep  Algorithm 

At  the  it  -  1st  sample,  the  cost-to-completion  J(k~  1)  is  given  by  the  following 
expression: 
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The  first  term  accounts  for  the  constraints  and  the  cost-to-completion  from  sample  k.  The 
second  term  accounts  for  the  control  cost  incurred  between  k  -  1  and  k,  while  the  third 
term  accounts  for  the  constraints  present  between  k  -  1  and  k.  The  quantity  6(£)  is  the 
scale  factor  matrix  for  the  state-control  constraint.  Because  all  of  the  rows  of  the 
state-control  constraints  are  constraint  rows,  6(*)  is  a  qk  x  <7*  zero  matrix  and  therefore  its 
presence  in  the  cost-to-completion  must  be  interpreted  according  to  (3.46).  Intuitively,  this 
representation  can  be  interpreted  as  a  penalty  function  on  constraint  violations  where 
violations  of  the  constraints  yields  infinite  cost.  It  should  be  noted  and  emphasized  that  the 
actual  inversion  of  the  scale  factor  matrix  is  never  necessary.  The  update  equation  for  the 
scale  factor  matrix  serves  as  a  mechanism  for  communicating  information  about  future 
constraints  backwards  to  previous  samples.  The  inclusion  of  the  constraints  in  this  manner 
is  similar  to  the  approach  suggested  by  Golub  and  Van  Loan  [15]  for  solving  constrained 
least  squares  optimization  problems. 

The  optimal  cost-to-completion  is  given  by 

J*{k  -  1)  =  min  !./*(*)  +  \  6uT(*  -  1)R(*  -  l)8u(Jfc  -1)4-  r^JD'1^)  r (*)) 

8^-1)  (3.49) 
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By  using  the  square  root  sweep  parameters  at  k ,  the  cost-to-completion  can  be  written  as 

J*(k  -  1)  =  min  ^  ($(£)  +  zT(k)D  l{k)2(k)  +  -  l)R(ik  -  l)8i4fc  -  1)  +  r^ikjD*1^)^)) 

W-l) 


(3.50) 


Equation  (3.50)  is  similar  to  (3.1 1)  in  the  previous  section. 

Similar  to  the  previous  section,  the  cost-to-completion  can  be  factored  into  the 
following  square  root  form 

J*(k-  1)=  min  ^  {$(&)  +zr(fc)D‘1(&j2^)} 

&<*-!) 

where 

^*)  =  \\{*)53(*)  +  v{*) 


(3.51) 


(3.52) 


and 


W{k)  0 

0  YR(*-1) 

«(*)  q*- 1)  . 


m= 


D {k)  0 
0  I 
0  0 


0 

0 

0 


(3.53) 


(3.54) 


\{k) 

0 


L  A(A:  -  1)J 


(3.55) 


5x(/k)  = 


5  <k) 

bt(k  - 1)  . 


(3.56) 
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In  writing  D(£),  the  zero  scale  factor  matrix  1 3(*)  associated  with  the  state-control 
constraints  are  explicitly  shown  in  the  lower  right  hand  partition  of  D(k).  The  rows  of 
(3.53)  have  the  following  interpretations 

1)  The  first  n  rows  account  for  future  constraints  and  the 
cost-to-completion  from  sample  k  to  the  end  of  the 
problem. 

2)  The  second  m  rows  account  for  the  control  cost 

associated  with  the  transition  from  sample  k-  1  to 
sample  k. 

3)  The  last  n  rows  account  for  the  state-control  constraints 

present  between  sample  k  -  1  and  sample  k. 

The  quantities  in  (3.53-3.55)  define  a  set  of  augmented  square  root  sweep  parameters. 

The  problem  shown  above  shares  many  similarities  with  the  square  root  solution 
developed  in  Section  3.2.3.  For  example,  the  matrix  W (it)  in  (3.53)  represents  an  enlarged 
square  root  matrix  similar  to  W (k)  in  (3.23).  Also  an  approach  to  the  determination  of  the 
control  perturbation  8u(Jfc  -  1)  that  minimizes  the  cost-to-completion  is  not  readily 
apparent.  The  problem  in  the  previous  section  can  be  interpreted  as  having  an  identity 
matrix  as  its  scale  factor  matrix  and  no  state-control  constraints.  Similar  to  the  problem 
considered  in  the  previous  section,  a  transformation  analogous  to  T(it)  in  (3.26)  is  used  to 
determine  the  control  perturbation  8u(ik  -  1).  The  transformation  needs  to  preserve  the 
cost-to-completion,  "shrink"  the  augmented  square  root  sweep  matrix  W (k)  and  permit  the 
determination  of  8u(Jfc  -  1).  The  transformation  used  for  the  current  problem  will  differ  in 
several  respects  from  the  orthogonal  transformation  used  in  Section  3.2.3.  Because  of  the 
presence  of  constraints,  the  transformation  must  preserve  and  communicate  constraint 
information  to  earlier  samples.  The  mechanism  for  doing  this  is  the  transformed  scale 
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factor  matrix  and  the  transformed  square  root  sweep  matrix.  The  transformed  scale  factor 
matrix  will  indicate  which  rows  of  the  transformed  square  root  sweep  matrix  will  be 
constraint  rows  for  the  next  update. 

To  facilitate  the  computation  of  the  optimal  control  and  the  backward  recursions  for 
the  square  root  sweep  parameters,  a  Householder  transformation  is  introduced.  The 
transformation  used  in  the  current  problem  differs  from  the  traditional  Householder 
transformation  defined  in  [**] 

Definition  of  a  Householder  Transformation 

Let  D  be  an  n  x  n  diagonal  matrix  with  nonnegative  entries.  An  nxn  matrix  A  will 
be  called  a  Householder  transformation  for  D  if  the  following  two  properties  are  satisfied: 

1)  A  is  nonsingular 

2)  D  =  ADAT  is  diagonal 

The  above  definition  places  rather  modest  restrictions  on  the  Householder  transformation 
A.  The  derivation  of  the  update  equations  for  the  square  root  sweep  parameters  places 
further  restrictions  on  the  Householder  transformation  A.  Several  useful  properties  of  the 
Householder  transformation  will  be  employed  to  derive  the  backward  recursions  for  the 
square  root  sweep  parameters. 

Lemma 

The  matrix  D  has  the  same  number  of  nonzero  diagonal  elements  as  D  and  the 
diagonal  elements  of  D  are  nonnegative. 

Proof: 

By  definition,  the  matrices  D  and  D  are  congruent.  Let  A  be  a  Householder 
transformation  for  D.  Because  the  Householder  transformation  A  is  nonsingular,  rank( D) 
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=  rank(D).  Hence  since  D  and  D  are  diagonal  matrices,  they  have  the  same  number  of 
nonzero  diagonal  elements.  Now  consider 

x^Dx  =  xtADAtx 

=yT°y  0.57) 


where  y  =  Ax.  By  assumption,  D  £  0  (i.e.  yTDy  £  0).  Since  A  is  nonsingular,  y  =  0  if 
and  only  if  x  =  0.  Therefore,  D  >  0  and  the  diagonal  elements  of  D  are  indeed 
nonnega  tiveM 

The  first  part  of  the  previous  lemma  is  simply  a  statement  of  Sylvester’s  law  of  inertia  [16]. 
Theorem 

Let  Abeanxn  Householder  transformation  of  the  n  x  n  nonnegative  diagonal 
matrix  D  and  let  W  be  an  n  x  n  matrix.  Define  the  n  x  n  matrix  W  in  the  following  manner: 


W  sAW 


(3.58) 


Then 

(1)  The  subspace  spanned  by  the  rows  of  W  that  correspond  to  the  zero  diagonal 
elements  of  D  is  the  same  subspace  spanned  by  the  rows  of  W  that  correspond  to  the  zero 
diagonal  elements  of  D  (the  i-th.row  of  W,  w,^,  corresponds  to  the  i-th. diagonal  of  D,  dd- 

(2)  Let  W(T  denote  i-th  row  of  W.  If  w,^x  =  0  for  every  i  such  that  dt  =  0  then: 

i=  l 

d{*  o 


X 

i=  1 
1*0 


~T 

W 


rx 


Proof: 
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Without  loss  of  generality,  assume  that  the  rows  of  W  that  correspond  to  zero 
diagonal  elements  of  D  appear  first.  This  can  always  be  accomplished  by  transforming  W 
and  D  by  an  appropriate  permutation  matrix.  Also  the  Householder  transformation  A  can 
be  constructed  in  such  a  manner  that  the  rows  of  W  that  correspond  to  zero  diagonal 
elements  of  D  occur  first.  Hence  D  =  ADAT  becomes 


0  0 


Ml 


M2 


0  0 


kll 


L12 


0  D22 


a21  a22 

T 

Ai2D22A12 

T 

A22D22A12 


L  0  D22 

T 

Aj2D22A2j 

T 

A22l>22A22 


(3.60) 


where  D22  and  D22  are  the  positive  diagonal  submatrices  of  D  and  D  respectively. 
Therefore  Ai2D22ATi2  =  0  implies  that  A12  =  0  since  D22  >  0.  Therefore  A  is  a  block 
lower  triangular  matrix.  Because  A  is  a  Householder  transformation  and  by  definition  must 
be  nonsingular,  the  matrices  An  and  A22  must  be  nonsingular  to  ensure  the  invertibility  of 
A.  Applying  the  transformation  to  W  yields: 


W1 

An 

0 

w 

W2. 

.A21 

a22 

w, 

(3.61) 


The  rows  of  Wj  correspond  to  the  zero  diagonal  elements  of  D  and  the  rows  of  W2 
correspond  to  the  positive  diagonal  elements  of  D.  Similarly,  the  rows  of  Wj  correspond 
to  the  zero  diagonal  elements  of  D  and  the  rows  of  W2  correspond  to  the  positive  diagonal 
elements  of  D.  Since  An  is  a  nonsingular  matrix,  the  rows  of  Wi  span  the  same  subspace 
as  the  rows  of  Wj.  This  proves  the  first  part  of  the  theorem. 

By  assumption,  WiX  =  0  and  hence  from  (3.61)  W2X  =  A22W2X.  Now  consider  the 
following  scalar  quantities: 
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Q=  X  (w Jxfldi 


di* 0 


(3.62a) 


Q=  X  Wxf/d, 

i=  i 
di*0 


(3.62b) 


Because  the  nonzero  elements  of  D  appear  only  in  D22,  the  scalar  quantity  Q  can  be  written 


Q  =  xTW2D^W2x 


(3.63a) 


Similarly,  since  the  nonzero  elements  of  D  appear  only  in  D22,  the  scalar  quantity  Q  can  be 


written  as: 


Q  =  xTwj322W2x 


(3.63b) 


Since  Aj2  =  0,  (3.60)  becomes 


0  0  0  0 


0  D22J  L°  A22D22A22 


(3.64) 


1  T  1 

The  nonsingularity  of  A22  implies  that  D22  =  A  22^22^22-  Using  this  result  in  (3.63a) 


yields: 


Q  =  xtW2D^1W2x 
=  xtW^D221A22W2x 
=  xtw3>221W2x 


(3.65) 


Therefore 
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S  - X  Rxf/2, 

i  =  1  i  =  1 

d{*  0  d.*0 

which  completes  the  proof  of  the  theorem.  ■ 


(3.66) 


In  the  spirit  of  the  current  problem  the  following  observations  can  be  made.  The 
lemma  ensures  that  the  application  of  the  Householder  transformation  to  the  scale  factor 
matrix  D (k)  does  not  cause  the  constraint  information  contained  in  the  scale  factor  matrix  to 
be  lost.  Furthermore,  the  first  part  of  the  theorem  guarantees  that  the  information  in  the 
constraint  rows  of  the  square  root  sweep  matrix  W (k)  is  not  lost  by  the  application  of  the 
Householder  transformation  (see  (3.58)).  Finally,  the  second  part  of  the  theorem  ensures 
that  the  value  of  the  cost-to-completion  will  be  preserved  by  the  application  of  the 
Householder  transformation  (see  (3.59)). 


To  complete  the  derivation  of  the  square  root  sweep  parameters,  a  Householder 
transformation  will  be  applied  to  the  augmented  square  root  sweep  parameters  (3.53-3.55). 
To  achieve  this  goal,  a  Householder  transformation  referred  to  here  as  A(Jfc)  needs  to  satisfy 
the  following  requirements: 

(1)  The  last  qt  rows  of  A(jfc)W(*)  must  be  zero  and  the  last 
qt  diagonal  elements  of  A(k)  D(k)AJ(k)  unity. 

(2)  The  first  n  rows  of  A(k)Y/(k)  must  be  orthogonal  to 
[GT(*-1)  nT 

The  matrix  A(Jfe)  plays  a  role  similar  to  the  matrix  T (k)  in  the  previous  section.  The  details 
of  the  construction  of  A (k)  are  discussed  in  Section  3.5.  The  first  requirement  is  necessary 
to  determine  the  square  root  sweep  matrix  update  and  the  cost-to-completion,  while  the 
second  requirement  is  necessary  to  determine  the  optimal  value  of  8u(Jfc  -  1). 
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Assume  that  the  Householder  transformation  A (k)  makes  the  last  <7*  rows  of 
A(k)W(k)  zero.  It  will  now  be  shown  that  the  last  <7*  diagonal  elements  of  A(k)D(k)AJ(k) 
are  nonzero.  By  the  first  part  of  the  theorem  on  Householder  transformations,  the 
constraint  rows  of  W(Jfc)  and  A(k)W(k)  span  the  same  subspace.  The  constraint  rows  of 
W (k)  are  assumed  to  be  linearly  independent,  therefore  the  subspace  spanned  by  the 
constraint  rows  has  the  same  dimension  as  the  number  of  constraints.  Because  the  last  <7* 
rows  of  A(k)W(k)  are  zero,  none  of  them  can  be  a  constraint  row.  Otherwise,  the 
dimension  of  the  subspace  spanned  by  the  constraint  rows  would  be  less  than  the  number 
of  constraint  rows.  Since  none  of  these  rows  is  a  constraint  row,  the  associated  scale 
factors  must  be  positive.  If  the  resulting  scale  factors  are  not  unity,  it  is  a  straightforward 
matter  to  compute  a  diagonal  nonsingular  transformation  that  will  render  them  unity  while 
maintaining  the  other  required  properties.  This  analysis  ensures  that  the  constraint  rows  of 
A(k)W(k)  are  linearly  independent  and  hence  the  first  requirement  listed  above  will  be  met 

Let  z(k)  =  A(k)z(k).  By  the  second  part  of  the  theorem  on  Householder 
transformations,  the  following  expression  is  true 


fl  +  W  +  n  T  Wt%  T  «f  1  . 

X  ziWd,(*)=  X 

<- 1 

di(k)*0 


n  +  m  +  qt 

I  *• 

<«  1 

*,(*)*  0 


(*) 


(3.67) 


The  (n  +  m  +qt)  x  (n  +  m  +<7*)  Householder  transformation  A(Jt)  can  be  written  as  a 
3x3  block  matrix  of  the  form: 


m 


^ll(^)  A12(£)  ^13^) 
A2i(k)  A 22(£) 

A31(&)  A32(k)  A33(fc) 


Therefore  from  (3.68)  and  (3.52-3.56)  z(it)  can  be  written  as: 


(3.68) 


1 

J 
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J(*-1)  AU(*)A12(*)A  13(t) 

r{k)  =  Zj(4)  =  A^^)  A22(4)  A23(4) 

z2^)  .  A3i(/:)  A32(A)  A33(4) 


VK4)  0 
0  YR(4-  I) 

H(4)  0(^-1) 


6x(4) 
8u(4  -  1 ) 


W(4-  l)Wj(4-  1) 
W^-ljW^-l) 


5x(4) 
8t(4  - 1) 


A(4  -  1) 


An(4)v(4)+A13(4)A(4-1) 
+  A21(4)v(4)+A23(4)A(4- 1) 
A31(4)v(4)  +A33(4)A(4  -  1) 


(3.69) 


The  matrices  W (K  -  l),Wi(4  -  1),W2(4-  1)  and  W3 (4-  1)  in  (3.69)  are  defined 
implicidy  from  the  product  A(4)W(4).  The  dependence  of  (3.69)  on  8x(4)  can  be 
eliminated  by  substituting  (3.31)  into  (3.69).  Equation  (3.69)  then  becomes 


2(4-1) 

Zj(4) 

Hk) 


W(4-l)  W.(4-l)  r  - 

«H(4-1)G(4-1)  8x(4  - 1) 
W4k-  1)W3(4- 1)  n  w  *  ] 

*  ’  *  .  0  I  5t<4- 1) 


An{4)v(4)+A13(4)A(4-1) 
+  A21(4M4)+A23(4)A(4-1) 


A31(4M4)+A33(4)A(4-1) 


\M4-1>D(4-1) 


=  W2(4-I)d<4-1)  W2(4-1)G{4-1)  +  W3(4-1) 


An(4)v(4)+Ai3(4)A(4-  1) 
+  A2i(4)v(4)+A23(4)A(4-1) 
_A3i(4)v(4)  +A33(4)A(4-  1) 


Sx(4  -  1) 
8u(4-  1) 


(3.70) 
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where  the  orthogonality  requirement  of  the  first  n  rows  of  A(k)W(k)  with  [GT(k  -  1)  I]T 
has  been  invoked  to  zero  the  1,2  submatrix  operating  on 


8x(/k  - 1) 
8v(k  -  1) 


in  the  second  half  of  (3.70).  Using  (3.67)  the  cost-to-completion  can  be  rewritten  in  terms 
of  the  subvectors  of  z  (k)  (see  (3.69)  or  (3.70))  in  a  form  similar  to  (3.43) 


i  =  1 

di{k-  1)*  0 


>Kk-i )  I  f  iiw 

</.(*- 1)  ,r,  * 


(3.71 


where  di  (k  -  1)  and  di^Jt)  are  the  diagonal  elements  of  the  matrix: 

_  _  [  D(k  - 1)  0  O' 

D(*-l)=A(t)D(t)Ar(t).  0  D,(t)  0 

o  o  I  J  (3.72) 


Inspecting  the  subvectors  of  z(k)  in  (3.70),  reveals  that  the  subvector  zi (k)  alone  exhibits 
dependence  on  the  control  perturbation  8u(k  -  1).  In  order  to  minimize  the 
cost-to-completion  at  the  sample  k  -  1  (i.e.  J*(k  -  1)),  while  satisfying  state-control 
constraints  and  the  terminal  boundary  condition,  5u(Jfc  -  1)  is  chosen  to  be: 

8t ik  -  1)  =  - (W ik  -  \)G{k  -  1)  +  yVik  -  1))  -‘(W^  -  1  )Q{k  -\Mk~  1) 

+  A2\{k)v{k)  +  A23 (k)A{k  -  1))  (3.73) 

Choosing  8u(ik-  1)  in  accordance  with  (3.73)  results  in  a  zero  value  for  zi(Jfc).  The 
choice  of  (3.73)  is  analogous  to  the  choice  made  in  (3.28)  of  Section  3.2.3.  The  various 
terms  in  the  expression  for  the  control  perturbation  (3.73)  can  be  interpreted  in  the 
following  manner 
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1)  W2(Jfc  -  l)0(fc  -  1  )5x(k  -  1)  -  acts  as  a  state  feedback 
term  to  account  for  the  current  value  of  the  state,  to  improve 
performance  and  satisfy  future  constraints. 

2)  A2i(*)v(£)  -  acts  to  satisfy  future  state-control  constraints 
and  the  terminal  boundary  conditions. 

3)  A23(ik)A(ik  -  1)  -  acts  to  satisfy  the  state-control 
constraints  active  from  jfc  -  1  to  k. 


The  matrix  W  2(k  -  1)G  (it  -  1)  +  W  i(k  -  1)  is  an  invertible  matrix,  since 
rank(\lR(k  -  1))  =  m  and  since  the  matrix 


W[k)G(k  - 1) 

YR(*-1) 

H(*)G{*-l)  +  q*-l). 


(3.74) 


has  m  columns  then 


"]) 


=  m 


(3.75) 


Therefore  the  nonsingularity  of  the  Householder  transformation  A(k)  implies  that  the 
rank(W2(k  -  l)G(ik  -  1)  +  Wi(k  -  1))  =  m  since  it  is  the  only  nonzero  partition  of  the 
matrix 


11 


0 

wik-\Wk-\)+v/ik-\) 

0 


(3.76) 


This  fact  is  also  obvious  from  (3.70).  Choosing  5u (k-  1)  in  accordance  with  (3.73) 
makes  the  optimal  cost-to-completion 
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where  [  •  ]y-  denotes  the  ij  submatrix.  These  update  equations  are  solved  backwards  from 
the  final  sample  k  =  N  with  boundary  conditions  (3.45a-3.45d).  Equations  (3.79a-3.79d) 
provide  a  means  of  solving  the  constrained  discrete-time  linear  quadratic  optimization 
problem  (3.29-3.34).  It  is  important  to  note  that  the  value  of  the  state-control  constraint 
A(ifc-  1)  impacts  only  s(k)  and  y(k).  Equations  (3.79b-3.79c)  explicitly  show  the 
dependence  of  s(k)  and  \(k)  on  A (k  -  1). 
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The  constraint  rows  of  W {k  -  1)  in  (3.79a)  can  be  shown  to  be  linearly  independent 
Recall  that  the  constraint  rows  of  A(£)\V(&)  are  linearly  independent.  Since  the  diagonal 
blocks  of  the  2  x  2  block  upper  triangular  matrix 


<&(*  -  1)  G{k  -  1) 

0  I 


(3.80) 


are  nonsingular,  the  constraint  rows  of 


A(k)W{k 


'$(k-l)G{k- 

0  I 


(3.81) 


remain  linearly  independent  Therefore,  the  constraint  rows  of  [W(jfc  -  1)C>(A:-1)  0]  are 
linearly  independent.  Hence  the  constraint  rows  of  the  matrix  W(£  -  1  )0(it  -  1),  which 
is  just  the  square  root  sweep  matrix  W (k  -  1),  are  indeed  linearly  independent. 

3.3,5  Sweep  Parameter  Updates  for  Initial  Boundary  Conditions 

At  the  initial  sample,  the  initial  constraint  (3.32)  must  be  satisfied  and  any  unspecified 
initial  conditions  determined  so  that  the  cost-to-completion  from  the  initial  sample  to  the 
final  sample  is  minimized  Similar  to  (3.52)  the  quantity  z(0)  can  be  written  as 


z(0)=W(0)6x(0)+v(0) 


(3.82) 


where 


nw-f28 


(3.83a) 


D(0)  0 
0  0 


(3.83b) 


K0)- 


J  v(0) 


(3.83c) 
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represent  the  augmented  square  root  sweep  parameters  at  the  initial  stage.  W(0),  D(0)  and 
v(0)  account  for  the  cost-to-completion  from  the  initial  sample,  future  constraints  beyond 
the  initial  sample  and  the  initial  boundary  conditions.  At  the  initial  stage,  the  cost-to- 
completion  becomes  equivalent  to  the  total  control  cost7/;n 


s(0)+  j 

i=0 
dt{ 0)  #  0 


(3.84) 


The  total  control  cost  Jun  accounts  for  the  initial  boundary  conditions,  future  state-control 
constraints  and  the  terminal  boundary  conditions.  As  always,  the  constraint  rows  of  W(0) 
must  be  linearly  independent. 

Potter  claims  that  it  is  theoretically  possible  for  the  matrix  W(0)  to  be  rank  deficient 
[6].  The  implication  of  this  statement  is  that  some  of  the  initial  states  may  be  chosen  freely. 
Assume  that  the  rank(\V(0))  ~  n  <,  n.  To  account  for  the  initial  boundary  conditions,  a 
Householder  transformation  will  be  used  to  update  the  square  root  sweep  parameters.  Let 
the  Householder  transformation  A(0)  be  partitioned  in  the  following  fashion. 


A<0)  = 


All(O)  A,j(0)l 
A21(0)  A22(0) 


n 

n  +  r-n 


(3.85) 


The  Householder  transformation  A(0)  is  constructed  such  that 


MO)y\o)= 


n 

W, 

0 


n 

n  +  r-n 


(3.86) 


The  transformed  augmented  scale  factor  matrix  D(0)  becomes 


AfOJEKOJA^O)  = 


Di(0)  0 

0  I 


(3.87) 
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Applying  the  transformation  to  z(0)  yields: 

zi(0) 
Z2(0)_ 


Mom = 


so  that  the  cost-to-completion  becomes: 


i=  0 


4,(0)*  0 


(3.88) 


(3.89) 


Expanding  the  subvectors  zi(0)  and  Z2(0) 

zi(0)  =W18x(0)  +  A„(0M0)  +  A12(0)b(0) 

z^O)  =  A21(0)v(0)  +  A22{0)b(0)  (3.90) 

Jiin  is  minimized  if  5x(0)  is  chosen  to  make  zi(0)  =  0.  Since  rank(Wl)  =  n  £  n  this  is 
in  general  possible.  Therefore,  the  optimal  cost-to-completion  becomes: 

4,  =  j(4o)+Mof) 


(3.91)  gives  the  total  cost  for  the  entire  problem. 

3.4  Alternative  Control  Formulas 

In  Section  3.3,  a  solution  to  the  discrete-time  linear  optimization  problem  of  Chapter 
2  was  developed.  In  the  present  section,  several  alternative  methods  of  computing  the 
control  perturbation  8u(it)  will  be  derived.  These  perturbation  equations  may  exhibit 
superior  numerical  performance. 
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3.4.1  Lagrange  Multiplier  Computation 

Recall  from  the  necessary  conditions  in  Chapter  2,  that  the  control  perturbation  6u0t) 
can  be  computed  from  the  equation: 


8u  (*)  =  -  R  ~\k)[GT{k)\(k+\)  +  CT(*)p(*+l)] 


(3.92) 


Unfortunately,  the  Lagrange  multipliers  |i(jfc  +  1)  and  %(k  +  1)  are  unknown.  A  method 
for  computing  \i(k  +  1)  and  *h(k  +  1)  is  developed  in  the  current  section.  In  order  to 
compute  |i(fc  +  1)  and  $.(k  +  1),  a  variable  called  the  v-costate  is  introduced.  Knowing 
+  1)  and  k(k  +  1),  8u (it)  can  be  computed  from  (3.92)  or  directly  from  the  v-costate. 

The  Lagrange  multiplier  | i(k  +  1)  describes  the  impact  of  changes  in  the  value  of  the 
state-control  constraint  A(£)  on  the  total  cost.  From  (2A.1),  it  can  be  shown  that  p(&  +1) 
can  be  expressed  as: 


p(*+l)  = 


dA  (*) 


(3.93) 


A  change  in  the  value  of  the  state-control  constraint  A(k)  impacts  the  sweep  parameters, 
s(k)  and  v(£).  In  the  following  derivation,  the  update  equations,  (3.79b-3.79c)  used  are 
those  between  k  +  1  and  k.  Therefore,  the  value  of  p(ik  +  1)  can  be  written  as 


u tk  +  n-[._ g*  dsik)  ■  ^ 

[a^)aA(/fe)  dyik)dA{k) 


(3.94) 


Each  of  these  terms  can  be  further  simplified  by  employing  (3.79b-3.79c)  and  (3.91): 
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dJun  _  dJiin  3s(0)  ds(k  -  1) 
05(0)  ds(  1 )  ds(k) 


1 


(3.95) 


+ iM*  + 1)+ a33(*  *  i  W*f 

3A(fc)J  dA(k) 

=  2A73(*  +  i(a31(*  +  1M*  +  1)  +  A3  4k  +  1)A(*)] 

(3.96) 


^77=-4t(au(*+  1M*+  1)+ a,3(*+  lWt)) 

dA(k)  dA  (k) 

=  A13(*  +  1) 

(3.97) 


Substituting  these  results  into  (3.94)  yields  the  following  formula  for  p(jfc  +  1) 


\i{k  +  1)  =  A]2{k  +  l{A31(*  +  l)v(*  +  1)  +  A33(*  +  1)A  (*)]+  AJn(k  +  1 


7Vnf 


97/ 


Mk) 


(3.98) 


Equation  (3.98)  will  be  useful  as  a  means  of  computing  )i(k  +  1). 

A 

Again  from  (2A.1),  the  Lagrange  multiplier  Mk)  describes  the  impact  of  perturbations 
in  the  state  hx(k)  on  the  total  cost  7/,„ .  Let  5x(Jfc)  =  5 x(k)  +  q,  then  X(k)  can  be  expressed 
in  the  following  manner: 


m- 


(3.99) 


Although  (3.99)  provides  a  means  of  computing  the  value  of  \(k),  the  value  of  *k(k  +  1 ) 
can  be  easily  computed  using  (2.55)  in  Chapter  2  and  the  invertibility  of  O(fc).  Consider 
the  impact  of  changing  only  8 x(k)  between  samples  k  and  k  +  1.  Therefore 
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i (k)  =  W[k)bx(k)  +  v(ik) 


(3.100) 


becomes 


z (k)  =  VJ{k)Sx{k  )  +  \M*)n  +  v(*  ) 


(3.101) 


The  only  sweep  parameter  that  gets  changed  is  \(k).  Specifically, 


v(fc)  =  \{k)  +  y\k)t] 


(3.102) 


Equation  (3.102)  should  be  interpreted  as  an  update  or  replacement  and  not  as  an  equation. 
Using  this  result,  the  Lagrange  multiplier  can  be  computed  in  the  following  fashion. 


[Mk)  dq  J 


(3.103) 


3.4.2  Computation  of  the  v-Costate 

Computing  both  X(k)  and  p.(Jfc  +  1)  requires  the  computation  of  the  quantity 


dJ  lin 

Hk). 


(3.104) 


This  quantity  can  be  interpreted  as  a  sensitivity  that  describes  the  impact  of  changes  in  the 
value  of  v(Jfc)  on  the  total  cost  (i.e.  the  cost-to-completion  at  the  initial  sample).  Hence  the 


following  definition  will  be  made: 


dJ lin  T 

.Mk). 


(3.105) 
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The  quantity  \\f(k)  will  be  called  the  v-costate  since  it  specifies  the  sensitivity  of  the  cost  to 
changes  in  the  value  of  v(Jfc).  From  the  update  equations  for  the  sweep  parameters 
(3.79a  -  3.79d),  it  is  obvious  that  changes  in  the  value  of  v(k)  will  impact  the  sweep 
parameters  v(fc  -  1)  and  s(k  -  1).  Therefore,  (3.105)  becomes 


dJlin  Bvjk-  1) 
dv(k-l)  dv(k) 


dJlin  bsjk-  1) 


ds(k-  1)  d\(k)  J 


=  AT  -  1)  +  AUkUlAkW)  +  A33(*)A(*)) 


(3.106) 


The  last  equation  provides  a  means  of  determining  y(jfc)  via  forward  propagation.  The 
boundary  condition  for  \y( 0)  is  found  from  (3.91)  and  is  given  by: 


V(°)  =  A2!(0XA2i(0)v(0)  +  A22(0)b(0))  (3. 107) 

Knowing  \\r(k),  the  values  of  p(£  +  1)  and  £(*  +  1)  can  be  determined  and  the 
value  of  8u (k)  found.  Alternatively,  the  control  perturbation  8u(k)  can  be  determined  from 
\}/(£),  \(k  +  1),  and  A(k  +1).  The  new  expression  for  5u (k)  is  given  by 


8u (*)  =  Alik  +  ljfAj,#  +1)  +  A33A (k  +  1)))  (3  108) 


A  derivation  of  this  expression  is  provided  in  Appendix  3A  of  this  chapter. 

3.5  Computation  of  the  Householder  Transformation 

Fundamental  to  the  development  of  the  backward  recursions  for  the  square  root 
sweep  parameters  has  been  the  introduction  of  the  Householder  transformation.  In  reality, 
the  transformation  used  in  the  current  problem  represents  a  generalized  Householder 
transformation  and  differs  from  the  classical  definition  given  in  [15,16].  The  major 
difference  arises  from  the  presence  of  the  scale  factor  matrix  and  its  associated  constraints. 
In  fact,  in  this  chapter,  the  Householder  transformation  is  defined  in  terms  of  its  action  on 
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the  scale  factor  matrix.  In  this  section,  a  computational  algorithm  for  computing  the 
Householder  transformation  to  meet  the  requirements  stipulated  in  section  3.3  is  presented. 
3.5.1  Transformation  Theory 

The  computation  of  the  Householder  transformation  can  be  broken  down  into  two 
distinct  stages.  First,  given  the  augmented  square  root  sweep  matrix  W (k)  and  the 
augmented  scale  factor  matrix  D (k),  compute  the  Householder  transformation  A \(k)  such 
that  the  last  qt  rows  of  Ai(J t)W(i)  are  zero.  A  by-product  of  this  computation  is  that  the 
transformation  leaves  the  remaining  rows  in  upper  triangular  form.  Second,  given  the 
transformed  augmented  square  root  sweep  matrix  Ai(k)W(it)  and  the  transformed  scale 
factor  matrix  A iOfc)D(fc)Ai(Jk)T,  compute  an  (n  +  m)  x  (n  +  m)  Householder 
transformation  A2(k)  such  that  the  first  n  rows  of 


Aik)  0 
0  I 


A, (two 


* 

(3.109) 


are  orthogonal  to  the  columns  of 


G<*-1)1 

1 


(3.110) 


Note  that  the  matrices  A2 (k)  and  A\(k)  are  both  Householder  transformations.  Hence  the 
product: 


A(*)  = 


Hk)  0 

0  I 


(3.111) 


is  a  Householder  transformation. 

Given  annxn  scale  factor  matrix,  the  problem  discussed  in  the  previous  paragraph 
can  be  stated  succinctly  as  the  following  general  mathematical  problem: 
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(1)  Given  an  n  x  k  matrix  X  such  that  k  <  n,  construct  an 
n  x  n  Householder  transformation  A  such  that  the  last  n  -  k 
rows  of  AX  are  zero. 

(2)  Given  annx«  matrix  X  and  an  n-dimensional  vector 
b,  called  the  orthogonality  vector,  construct  an  n  x  n 
Householder  transformation  A  such  that  n  -  1  rows  of  AX 
are  orthogonal  to  b.  The  nonorthogonal  row  of  AX  will  be 
called  the  reference  vector. 

Case  (1)  will  be  shown  to  be  a  simplified  version  of  Case  (2). 

3.5.2  Transformation  Algorithm 

Computation  of  the  Householder  transformation  is  accomplished  by  operating  on  two 
rows  of  X  and  the  associated  scale  factors  at  a  time.  Typically,  one  of  these  rows  serves  as 
the  reference  vector.  In  Case  (2),  one  of  the  rows  of  X  is  chosen  to  be  nonorthogonal  to  b. 
Each  of  the  remaining  rows  is  then  transformed  to  be  orthogonal  to  b.  In  Case  (1), 
suppose  that  the  i-th  column  is  to  be  eliminated.  One  of  the  rows  of  X  will  be  chosen  as 
the  reference  vector.  For  case  (1),  the  orthogonality  vector  b  is  chosen  so  that  the  only 
non-zero  component  is  the  i-th  component.  The  remaining  rows  are  then  transformed. 
Because  the  remaining  rows  must  be  orthogonal  to  b,  the  i-th  component  of  each  row  will 
be  zeroed.  Next,  a  second  column,  the  j-th  is  chosen  for  elimination.  A  new  row  is 
chosen  as  the  reference  vector  and  the  orthogonality  vector  b  is  constructed  so  that  the  only 
non- zero  component  is  the  j-th  component.  Except  for  the  current  and  previous  reference 
vectors,  the  remaining  rows  are  transformed  so  that  the  j-th  component  is  zero.  This 
process  is  repeated  k  -  2  more  times.  Upon  completion,  there  will  be  n  -  it 
nonorthogonal  rows  and  k  zero  rows.  By  using  the  appropriate  permutation  matrix,  the 
n  -  k  nonorthogonal  rows  can  be  placed  in  upper  triangular  form.  The  following  point 


65 


Chapter  3  Square  Root  Sweep  Algorithm 


should  be  emphasized:  the  order  of  column  elimination  is  arbitrary.  Numerical 
considerations  may  be  important  in  determining  the  order  of  elimination. 

The  Householder  transformation  operates  on  two  rows  of  the  matrix  X  at  a  time. 
Without  loss  of  generality,  suppose  the  two  rows  of  interest  are  x,  and  x;.  In  fact,  without 
loss  of  generality,  the  two  rows  can  be  assumed  to  be  adjacent  to  one  another  since  a 
permutation  matrix  is  always  a  Householder  transformation.  The  important  elements  of  the 
Householder  transformation  A  that  act  on  x,  and  x;  are  contained  in  the  submatrix 


fl»i  ttij 

,aji  a)j. 


(3.112) 


while  the  remainder  of  the  Householder  transformation  is  just  the  identity  matrix. 
Somehow,  the  quantities  a  a,  a  y,  and  ajj  must  be  determined.  Applying  the 
transformation  to  the  scale  factor  matrix  D  will  only  alter  the  scale  factors  of  x,  and  x;  In 
order  to  be  a  Householder  transformation,  the  following  relationship  must  hold: 


d^l  +djafj  d^ft^  +  dja^j 
diOifiji  +  dja^j  diofi+djafj 


(3.113) 


and  therefore: 


0  -  d^  flj  i  +  dja^  (3  1 14) 

Strictly  speaking,  the  quantities  a*  fly,  fl;<  and  flj  need  only  be  chosen  to  satisfy  (3.1 14)  and 
the  requirement  that  the  Householder  transformation  be  nonsingular.  Unfortunately,  the 
requirements  of  Case  (1)  and  Case  (2)  will  further  restrict  the  choice  of  these  parameters. 
In  particular,  suppose  that  b  represents  an  orthogonality  vector  and  it  is  required  that  the 
j-th  row  of  AX  be  orthogonal  to  b.  The  j-th  row  of  AX  is  given  by 


66 


Chapter  3  Square  Root  Sweep  Algorithm 


*j  =  ajPi  +  ajptj 

Therefore  and  fly  must  be  chosen  so  that : 

xp  =  aJtxp  +  cijpifi  =  0 


(3.115) 


(3.116) 


Clearly  if  xp  =  0,  the  Householder  transformation  is  not  needed  and  can  be  an  identity 
matrix.  Equation  (3.1 16)  will  hold  if  the  following  choices  are  made  for  a y 

Oji--  k\Xp 

aS  =  k\xp  (3.117) 

where  ki  is  an  arbitrary  nonzero  constant.  The  constant  ki  cannot  be  zero  since  the 
resulting  Householder  transformation  would  be  singular. 

Substituting  these  choices  for  a;i  and  a*  into  equation  (3. 102)  results  in  the  following 
expression: 

-  dflj<.\xp  +  dflipxxjo  =  0  (3.118) 


If  d(  =  dj  =  0,  then  au  and  a^  may  be  chosen  arbitrarily  provided  that  the  choice  does  not 
render  the  transformation  singular.  If  d,  =  0  and  xp  =  0  the  choice  of  a„  and  fly  is  once 
again  arbitrary.  Otherwise  the  choice  of  a  a  and  Oy  can  be  made  such  that 

a*  =  k2djxp 

Oj  =  k2d{xp  (3.119) 


where  k2  an  arbitrary  nonzero  scalar.  The  constant  k\,  k2  should  be  chosen  to  ensure  good 
numerical  properties. 

3.5.3  Transformation  Example 

This  section  concludes  with  an  example  that  illustrates  the  transformation  technique. 
Let  the  following  data  be  given 
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'1101  [000' 

X=  0  1  0  D=  0  0  0 

.  0  0  1  L  0  0  1  - 


(3.120) 


The  objective  of  the  transformation  is  to  make  the  first  two  rows  orthogonal  to  the  vector  b. 
The  third  row  of  X  will  be  chosen  to  be  the  reference  vector,  (i.e.  X3  will  play  the  role  of  x* 
in  the  previous  discussion.).  The  second  row  of  X,  denoted  by  X2,  will  play  the  role  of  Xj. 
Orthogonalizing  the  second  row  requires  that  022,  £223,  32  and  033  be  chosen  in  an 
appropriate  manner.  From  the  previous  discussion,  023  and  a 22  are  chosen  to  be 

02 3  =  -k\X2b  =  -k\ 

a22  =  k\x2b  =  k\  (3.121) 


The  values  of  <232  and  <333  are  given  by 


£233  =  k2d2x2b  =  0 
£232  =  k2d\X2b  —  k2 


(3.122) 


Letting  Jfcj  =  1  and  k2  =  1,  the  resulting  Householder  transformation  is  given  by: 


1  0  O' 
0  1  -1 
0  1  0. 


(3.123) 


Applying  this  transformation  to  X  and  D  yields: 

'10  01  [0  0  O' 

AX  =  0  1-1  ADAt=  0  1  0 

-0  1  oJ  L  0  0  0  J  (3.124) 


Notice  that  the  scale  factors  of  the  second  and  third  rows  have  been  changed  by  the 
Householder  transformation. 

Now  to  complete  the  transformation,  the  third  row  of  AX,  which  will  be  denoted  by 
X3,  will  play  the  role  of  x<in  the  previous  discussion  and  the  first  row  of  AX,  which  will  be 
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denoted  by  X3,  will  play  the  role  of  x; .  The  transformation  of  these  rows  will  be  denoted 
as  E.  The  elements  eu,  *13.  £31  and  £33  must  now  be  chosen.  The  proper  choices  are 


eu  =  -k  iXjb  =  -*il.5 

£n  =  *iX3b  =  *i  (3.125) 

Since  the  scale  factors  of  the  first  row  and  third  row  of  AX  are  zero,  the  proper  choices  of 
e33  and  e$i  are: 


£33  =  ^2^2X3  b  =  £2 

632  =  ^dixib  =  *21-5  (3.126) 


Letting  k\  =  \  and  *2=1.  the  resulting  Householder  transformation  is  given  by: 


E  = 


'  1 
0 

.  1.5 


0  -1.5' 

1  0 
0  0  . 


(3.127) 


Applying  the  transformation  E  to  AX  and  ADAT  yields: 


EAX  = 

'  1  -.5  O' 

0  1  -1 

eadatet  = 

^0  0  o' 
0  1  0 

1.5  1  0 

.0  0  0. 

(3.128) 


Finally  multiplying  EAX  with  b  confirms  that  the  first  two  rows  are  orthogonal  while  the 
third  is  not. 


r  0 


EAXb  = 


0 

1.75. 


(3.129) 


3.6  Analytical  Example 

In  this  section,  the  square  root  sweep  algorithm  is  applied  to  a  simple  analytical 
example.  The  objective  will  be  to  minimize  the  quadratic  performance  index  shown  below 
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3 tin  =  \  2  ^ik) 


subject  to  the  following  dynamics 


8*1  {*  +  1) 

1 

1 

5*i(*) 

+ 

.5 

8*2(*  +  1). 

0 

_ 

1 

.5*2 (*). 

1 

84*) 


and  the  following  hard  equality  constraints  and  boundary  conditions 

8.x  i{0)  =  a 
8*i{l)=& 

8*i(2)  =  c 


(3.130) 


(3.131) 


5u(l)  = 


(3.132) 


In  its  present  form,  the  problem  is  underspecified  since  the  value  of  5*2(0)  is  not 
known.  This  quantity  along  with  5«(0)  must  be  determined.  The  sweep  parameters  are 
initialized  to 


VK2)  = 


TO 
.0  0 


(3.133) 


the  augmented  square  root  sweep  parameters  are 


VH2)  = 


riooi 

rooooi 

r-c" 

000 

m= 

0 100 

42)= 

0 

00 1 
100 

0010 

0000 

0 

-d 

(3.134) 


The  Householder  transformation  for  this  stage  is  given  by 
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A(2)  = 


2  0  0  -1 

0  10  0 
0  0  0  1 

0  0  -  1  i 


(3.135) 


Applying  the  Householder  transformation  to  W(0),  D(0)  and  v(0)  and  solving  for  the 
control  using  (3.66)  shows  that  8u(l)  =  d.  The  updated  square  root  sweep  parameters  are 


"W-fool  DOI-P  Ito-Fo* 


(3.136) 


the  augmented  square  root  sweep  parameters  are 


'2  2  0' 


'0000' 


000  fo.v  0100 

nni  D(1)-  ftfttn  *0) 


001 

100 


0010 

0000 


’  d-2c 
0 
0 

-b 


(3.137) 


The  Householder  transformation  for  the  transition  from  1  to  0  is 


A(l)  = 


1/6  0 
1/2  0 
1  0 
0  1 


(3.138) 


Solving  for  the  control  gives  the  following  expression  for  8a(0) 


8u(0)  =  -  4/38x2(0)  -  2/30  -  1/3 (d  -  2c) 


(3.139) 


The  updated  square  root  sweep  parameters  are  given  by 


wm  T  2/3  - 1/31  wo)  -  [°  °1  v(0)  -  ~lc)~b 

^°)“[  0  -1  J'W-loi] 


(3.140) 


the  augmented  square  root  sweep  parameters  are 
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'  2/3  -  1/3' 

000' 

-\/dd-2c)-b' 

\M0)  = 

0  -1 

1  0 

m= 

010 

000 

v(0)  = 

-\/2{d-2c)-b 

-a 

(3.141) 


The  Householder  transformation  is  given  by 


A(0)  = 


10  0' 
-3  0  2 
3  1  -2 


(3.142) 


Applying  the  Householder  transformation  to  W(0),  D(0)  and  v(0)  yields 


A(0)W{0)  = 


'2/3  V3" 
0  -1 
0  0 


A(0)D(0)Ar{0)  = 


000' 

000 

001 


A(0M0)  = 


-\/&d-2c)-b 
\/2{d-2c)  +  3b-2a 
-(d-  2c)  -4b  +  2a 


from  which  the  value  of  &t2(0)  is  determined  to  be 

5x2(0)  =  l/2(d  -2c)  +  3b-2a 


Substituting  &c2(0)  into  the  expression  for  5u(0)  gives 

Su(0)  =  -{d-2c)-4b  +  2a 


(3.143) 


(3.144) 


(3.145) 


(3.146) 


(3.147) 


The  state  trajectory  is  given  by 
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5*i(l)  =b 

Sjt2{  1)  =  -  1/2 {d  -  2c)  -  b 
8x\{2)  =c 

8*2(2)  =  -  l/2(^-  2c)  -  b  +  d  (3.148) 

Clearly  all  of  the  constraints  have  been  met 

3.7  Summary 

The  square  root  sweep  algorithm  has  been  derived  in  this  chapter.  The  algorithm 
computes  control  perturbations  that  minimize  a  quadratic  control  cost  subject  to  specified 
boundary  conditions  and  hard  equality  constraints  on  the  perturbations  in  the  state  and 
control  variables.  The  essential  steps  of  the  algorithm  are  outlined  below. 

Step  1)  Initialize  the  sweep  parameters  at  the  final  sample 
k  =  N 


\M  N)  = 


B(A01 

0 


D (AO- 


0  0 
.0  I 


v(N)  = 


m 

0 


s(N)  =  0 


Steps  2,  3  and  4  are  computed  for  k  =  1. 
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Step  2)  Form  the  augmented  sweep  parameters  W (k),  D (k), 
and  v(k).  If  constraints  are  present,  the  augmented  sweep 


parameters  are 


W{lc)  0 

W  k)  =  0  YR(*-l) 

h {k)  q*-i). 


"  D(*)  0  0 

D(*)  =  0  I  0 

0  0  0 


\{k)  =  0 

A(k  -  1) 

otherwise,  the  augmented  sweep  parameters  are  given  by 

0  1 

[  0 


1 


Step  3)  Compute  the  Householder  transformation  A(Jfc) 
using  the  technique  described  in  Section  3.5.  If  constraints 
are  present,  construct  A (k)  such  that 


\V(k-  1)  W,(/k-  1) 
A '(k)W(k)  =  W £k  -  1 )  W3(*  -  1 ) 
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MkmV?(k)  = 


IX*- 1) 
0 
0 


0 

D,(*) 

0 


0 

0 

I 


with 

W(Jfc  -1  )G(k-  1)  +  W1(it-1)  =  0 

otherwise,  if  no  constraints  are  present  A(k)  is  constructed 
so  that 

W(*-l)Wj(*-l) 

-  1)  W3(J k  -  1) 

D(it  -  1)  0 

0  D,(*) 

with 

W  (*  -1)G {*-  1)  +  Wj(*-1)  =  0 

Step  4)  Having  computed  the  Householder  transformation 
A(k),  the  values  of  s(k  -  1),  \(k  -  1),  W  (k  -  1)  and 
D(Jfc-l)  can  now  be  computed.  When  constraints  are 
present,  the  updates  to  the  sweep  parameters  are  given  by 

s(k-l)  =  s(k)  +  |a3i(*m*)  +A33(*)A(*  -  1  f 


A (k)W[k)  = 


v(*-  1)=  An(*)v(*)+A13(*)A(*-  1) 

WfA  -  1)  =  VP(A  -  1>U(A  -  1) 
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I**-l)  =  [A(*W*jAr(*i!11 

If  no  constraints  are  present,  the  updates  to  the  sweep 
parameters  are  given  by 

s(k-  1 }  =  s(k) 


\{k-l)=  An  (*)#) 

=  W*-l) 


D(t-l)  =  [A(Wt)Ar(*)l11 

Step  5)  Form  the  augmented  sweep  parameters  W(0),  D(0) 
and  v(0) 


o 


v(0)' 

«0)J 


Step  6)  Compute  the  Householder  transformation  A(0)  such 
that 


A(0)W(0)  = 


Wi 

0 


A(0)D(0)Ar(0)  = 


Di(0)  0 

0  I 


Step  7)  The  value  of  5x(0)  is  computed  by  solving  the 
equation 
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Wi8x(0)  +  Au{0)v(0)  +  AJ2(0)b(0)  =  0 
Step  8)  Initialize  the  v-costate  \y(0)  according  to  the  equation 
H<0) = Al,(0XA2i(0M0)  +  Aliom) 

Steps  9  and  10  are  computed  for  k  =  l,...,iV  -  1. 

Step  9)  Propagate  the  value  of  \\f(k)  forward  in  time.  If 
constraints  are  present,  the  propagation  is  given  by 

Mhh  -  1)  +  AL(*j(A3i(*)v(*)  +  A33(k)A(k  -  1)) 

If  no  constraints  are  present,  the  propagation  is  given  by 

V(kh  1) 

Step  10)  If  constraints  are  present,  the  control  perturbation 
is  computed  from  the  formula 

5u (k)  =  ifR^A +  1M*)  +  A^  +  \U3^k  +  1)  +  A33A(A:))) 

If  no  constraints  are  present,  the  control  perturbation  can  be 
computed  from  the  formula 

8u(*)=VipHAL<*  +  iM*)) 

Once  5u (k)  is  known,  the  nominal  control  u (k)  can  be  updated  and  the  new  trajectory 
determined.  With  the  new  trajectory  known,  the  impact  of  the  control  perturbations  on  the 
constraints  and  performance  can  be  assessed. 
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Appendix  3A 

In  this  appendix,  the  derivation  of  (3.108)  is  given.  Recall  that  the 
perturbations  can  be  computed  from  the  formula 


8u(*)  =  -  R  -\k)[GT{k)X(k+l)  +  CT(*)p(*+l)] 


However, 


(2.55)  and  (3.79a)  imply  that 


/s 

X(k+\)  =  wty  M*) 


At  it  +  1,  the  value  of  p(it  +  1)  can  be  computed  from 

Vik  +  1)  =  +  i{a31(A  +  1M*  +  1)  +  A  lik  +  1)A(*)]  +  A ]3{k  +  1 M*) 


so  that 

5i {*)  =  -R  -'(*(gt(*)W(*  +  1)  +  +  l)jV*) 

-  R  \k)C?[k)Alj,k  +  l(A31(*  +  1M*  +  1)  +  Aj#  +  1)A(*)] 


From  (3.69), the  expression  for  A(k  +  l)W(jt  +  1)  the  following  conditions  hold: 

W,(it)  =  Aiik  +  l)lf1pj  +  Aiik  +  1)0 {k) 

0  =  A-iik  +  l)lf]py  +  A-$ik  +  \)ftk) 


W{*)G{*)  +  Wi(*)  =  0 


control 

(3A.1) 

(3A.2) 

(3A.3) 

(3A.4) 

(3A.5) 

(3A.6) 

(3A.7) 

(3A.8) 
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Substituting  (3A.8)  into  (3A.6)  yields 

Aii*  +  i)^=-(u<,t)c(*)  +  A,3<*  +  ijqt))  (3A.i0) 


Solving  (3A.7)  gives 


C^(k)AT^{k  +  1)  =  -  VR^A^*  +  1) 


(3A.11) 


Substituting  (3A.10)  and  (3A.1 1)  into  (3A.5)  gives  (3.108)  which  is  shown  below 

8i (k)  =  VR^-^A^/t  +  1  +  A T^k  +  1<A31(*  +  l)v(*  +  1)  +  A  33(*  +  1  )A(A: ))} 

(3A.12) 
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4  Simple  Applications  of  the  Square  Root  Sweep 

Technique 

4.1  Introduction 

In  this  chapter,  the  square  root  sweep  algorithm  developed  in  Chapter  3  is  applied  to 
two  simple  examples.  The  simple  examples  illustrate  the  capabilities  of  the  proposed 
algorithm  while  keeping  computational  requirements  to  a  minimum.  The  simple  examples 
are  helpful  in  developing  insight  into  the  choices  of  various  user  specified  parameters. 
Additionally,  several  mechanisms  for  enhancing  algorithm  convergence  can  be  easily 
illustrated  by  the  simple  examples. 

4.2  Example  1 

The  first  example  discussed  in  the  chapter  is  drawn  form  Bryson  and  Ho  [18].  The 
solution  of  this  problem  will  serve  several  purposes.  First,  it  will  show  the  viability  of 
using  the  proposed  algorithm  to  solve  optimization  problems  with  inequality  constraints. 
Because  the  optimal  solution  is  known,  the  solution  obtained  from  the  square  root  sweep 
algorithm  can  be  compared  to  the  known  solution.  It  should  be  noted  that  the  two  answers 
differ  slightly  since  the  square  root  sweep  solution  generates  an  approximate  discrete 
solution  to  the  continuous  time  problem.  Second,  it  will  illustrate  how  to  use  the  algorithm 
to  solve  a  simple  example.  Because  of  the  linearity  of  the  system  dynamics,  a  technique 
called  thresholding  is  used  in  solving  the  problem. 
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The  system  dynamics  for  this  problem  consist  of  two  linear  ordinary  differential 
equations: 


r(t)=v  (r) 

(4.1) 

v(t)  =  u  (t) 

(4.2) 

where  r(t)  and  v(t)  are  the  state  variables  and  u(t)  is  the  control  variable.  The  boundary 
conditions  on  the  system  are  as  follows: 


r(0)  =  r(l)  =  0 

(4.3) 

v(0)=- v(l)=  1 

(4.4) 

The  performance  measure  for  this  problem  is  to  minimize 

Jnl  =  \[ 

Jo  (4.5) 

In  order  to  use  the  algorithm  developed  in  Chapter  3,  the  performance  measure  in  (4.5) 
must  first  be  converted  into  a  Mayer  performance  measure.  Specifically,  let 

=  *°>  =  0  (4.6) 

Therefore,  the  objective  is  to  minimize: 

Jnl  =  z(1)  (4.7) 

Equations  (4.6)  and  (4. 1-4.2)  constitute  the  system  dynamics.  The  introduction  of  (4.6) 
causes  the  dynamics  to  become  nonlinear.  However,  the  nonlinearity  is  decoupled  from 
(4. 1-4.2). 
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A  single  hard  inequality-constraint  will  be  imposed  on  the  problem  and  is  given  by 

r(/)-.l£0  (4.8) 

This  constraint  acts  to  limit  the  excursions  of  the  state  variable  r(r)  from  the  origin.  The 
hard  inequality  constraint  in  (4.8)  represents  a  second-order  state  variable  inequality 
constraint.  Traditional  solution  techniques  would  require  that  (4.8)  be  differentiated  with 
respect  to  time  until  explicit  dependence  on  the  control  variable  u(t)  is  attained.  The 
algorithm  developed  in  Chapter  3  does  not  explicitly  require  that  the  constraint  be  handled 
in  this  manner. 

To  make  the  notation  compatible  with  Chapters  2  and  3,  define  the  following 
variables 

xi{t)  =  r(t) 

xi  (0  =  v<r) 

*3  WMO 

In  terms  of  (4.9),  the  problem  is  to  minimize 

Jnl-  f«l),l)  =  *3(l) 
subject  to  the  following  constraints: 

x(r)  =  f WO, 440 

or 

X2(0=Mx(t)Mt),t)=u(t) 

X3(t)=f3(x(t)Mt)'t)  =  lu2(t) 


(4.9) 


(4.10) 


(4.11a) 


(4.11b) 
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with  initial  constraints: 

q[x(0),0]  =  0  (4.12a) 


or 


*i(0)  =  0 

*2(0)  -  1=0 
*3(0)  =  0 

terminal  constraints: 

m[x(l),l]  =  0 


or 


inequality  constraint 


*i(l)  =  0 

x2(l)+  1=0 


or 


(4.12b) 


(4.13a) 


(4.13b) 


(4.14a) 


*iM-l£0  (4.14b) 

These  equations  form  the  basis  of  a  continuous  time  nonlinear  optimization  problem  with 
hard  inequality  constraints. 


In  order  to  solve  the  problem  formulated  in  this  chapter,  a  discrete  nominal  trajectory 
needs  to  be  postulated.  In  the  absence  of  the  hard  state  variable  inequality  constraint 
(4.16),  the  optimal  control  is  given  by  «(/*)  =  u(k)  =  -  2.0.  This  control  can  be  used  in 
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a  numerical  integration  of  the  system  dynamics  to  determine  x(r*)  =  \(k), 
<I>(f*  +  i,  tt)  =  0>(k)  and  G(r*  +.  j,r*)  =  G(k).  Figure  4.1  depicts  the  nominal  trajectory 
for  the  state  variable  *i(r). 


posmcw 

Constraint 


Time  (s) 

Figure  4.1  Example  1  Nominal  Plot  of  *i(r) 


Time  <•) 

Figure  4.2  Example  1  Nominal  Plot  of  *2(0 
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The  trajectory  shown  in  Figure  4.1  violates  the  hard  inequality  constraint  (4.14b).  The 
nominal  state  trajectory  for  the  state  variable  x2 (f)  is  shown  in  Figure  4.2. 

The  values  of  B  (N),  B(0),  b (N)  and  b(0)  can  all  be  computed  based  upon  the 
knowledge  of  the  nominal  state  x(k).  For  this  problem  B(/'/)  is  simply: 

‘  1  0  O' 

B(A0=  0  1  0 

-0  0  1J  (4.15) 


The  value  of  the  terminal  constraint  is  given  by: 

*i  (N) 

b (N)=  1  +  x2{N) 
cjxi(N) 


(4.16) 


It  may  be  desirable  not  to  constrain  the  performance  during  every  iteration.  This  may  occur 
when  one  would  only  like  to  improve  boundary  condition  and  constraint  satisfaction.  If 
this  occurs,  the  third  row  in  (4.15)  and  4.16)  is  not  included. 

Since  the  initial  state  is  entirely  fixed  and  hence  no  perturbation  8x(0)  is  allowed, 
B(0),  is  given  by 


10  0' 

B(0)=  o  1  0 

.  0  0  1  J  (4.17) 


The  value  of  the  initial  constraint  is  given  by 


(4. IS) 


When  the  inequality  constraint  is  violated,  an  equality  constraint  will  be  imposed  on  the 
discrete-time  optimization  problem.  Specifically,  the  values  of  H(*  +  1),  C (k)  and  &(k) 
are  given  by: 
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H(fe+  1)  =  [  1  0  0] 

q*)=o 

AM  =  x,(*+l)-.l  (4.19) 

Because  the  hard  inequality  constraint  is  not  an  explicit  function  of  the  control,  C(k)  =  0. 
The  equality  constraint  is  only  active  on  the  discrete-time  linear  quadratic  optimization  when 
the  hard  inequality  constraint  is  violated. 

All  the  information  necessary  to  apply  the  square  root  sweep  method  is  now 
available.  The  results  of  applying  the  square  root  sweep  method  are  shown  in 
Figures  4.3, 4.4  and  4.5.  Although  not  exact,  the  discrete  approximation  obtained  using 
the  square  root  sweep  algorithm  closely  approximates  the  optimal  solution.  The  value  of 
the  performance  measure  for  the  trajectory  shown  is  4.49  verses  4.44  for  the  optimal 
trajectory  computed  analytically  in  [18]. 


CCKTHOL 
Nominal  Control 


■  ■  ■  ■  i  i  i  i  i  i  i 

0.0  0.2  0.4  0.6  0.8  1.0 

Time  (») 


Figure  4.3  Example  1  Solution  for  u(t) 


Position 
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POSITION 
Nominal  Position 
Constraint 


0.0 

0.2  0.4  0.6  0.8  1.0 

Time  (s) 

Figure  4.4  Example  1  Solution  forxi(r) 

■  VELOCITY 

-------  Nominal  Velocity 

0.0  0.2  0.4  0.6  0.8  1.0 

Tima  (*) 

Figure  4.5  Example  1  Solution  for  *2(1) 

In  order  to  obtain  the  solution,  a  technique  called  thresholding  w..s  used.  In 
computing  the  solution,  several  interesting  observations  were  made.  When  constraint 
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violations  are  present,  the  square  root  sweep  technique  generates  a  solution  that 
shortsightedly  focuses  on  improving  constraint  performance  with  almost  total  disregard  for 
performance.  The  outcome  of  this  is  better  constraint  satisfaction  at  the  expense  of  a  higher 
cost  On  the  other  hand,  when  constraint  violations  are  not  present,  or  at  least  "small",  the 
square  root  sweep  techniques  places  greater  emphasis  on  performance.  In  this  situation, 
the  constraint  satisfaction  becomes  poorer  while  the  cost  is  improved.  It  is  believed  that  the 
near  linearity  of  the  model  is  the  source  of  this  behavior. 

To  overcome  this  difficulty,  a  threshold  was  applied  to  the  constraints.  The  basic 
idea  behind  thresholding  is  to  define  what  is  meant  by  a  small  violation  of  the  constraint. 
In  this  problem,  constraints  were  only  active  when  the  value  of  the  constraint  violations 
exceeded  some  threshold.  That  is  the  equality  constraint  was  only  active  when: 

X\(k)>  .1  x  constant  (4  20) 

The  value  of  constant  was  chosen  so  that  during  early  iterations,  constant  was  slightly 
larger  than  one  (e.g.  constant  =1.1  was  the  initial  value  used  for  this  example).  As  the 
solution  progressed,  the  value  of  constant  was  periodically  reduced.  Ideally,  one  would 
like  to  allow  constant  — » 1.  Part  of  the  reason  for  applying  thresholding  is  that  given  finite 
precision  computation,  it  may  be  difficult  to  exactly  satisfy  the  constraints. 

Several  heuristics  were  also  developed  during  the  course  of  this  work.  Specifically, 
there  appears  to  be  a  tradeoff  between  optimal  performance  and  constraint  satisfaction.  In 
computing  the  solution  it  was  sometimes  necessary  to  allow  the  cost  constraint  to  be 
inactive.  The  cost  constraint  is  contained  in  the  third  row  of  B  (N)  and  b (N).  When  the 
cost  constraint  is  inactive,  the  third  row  is  not  present.  Typically,  this  will  result  in  a 
modest  increase  in  the  value  of  the  performance  measure  that  is  being  minimized  with  a 
resulting  improvement  in  the  constraints.  Determining  when  it  is  necessary  to  inactivate  the 
cost  constraint  is  highly  problem  dependent.  The  technique  outlined  above  is  ad  hoc. 
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4.3  Example  2 

The  second  example  illustrates  the  use  of  the  technique  on  a  slightly  more  complicated 
example.  The  system  dynamics  for  this  problem  consist  of  a  second-order  nonlinear 
system: 


r(r)=v(r) 


(4.21) 


v(,)  =  -lv  *(,)«<,) 


(4.22) 


where  r(r)  and  v(r)  represent  the  state  variable  and  u(t)  is  the  control  variable.  These 
equations  describe  the  motion  of  a  simple  Newtonian  cart  with  drag  impeding  the  motion  of 
the  cart  The  boundary  conditions  on  the  system  are  as  follows: 


K0)  =  0  r{  1)=1 


(4.23) 


v(0)  =  0  v(l)  =  0 


(4.24) 


The  performance  measure  for  this  problem  is  to  minimize: 


■if- 


(4.25) 


Similar  to  the  previous  example,  the  performance  measure  must  first  be  converted  into  a 
Mayer  form.  Specifically,  let 


HO = ^HO  HO) =o 


(4.26) 


Once  again,  the  objective  is  to  minimize 


Jnl  =  A  1) 


(4.27) 


A  single  hard  inequality  constraint  will  be  imposed  on  the  problem  and  is  given  by 
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w  =  lv2(t)-l  <0 


(4.28) 


The  hard  inequality  constraint  resembles  a  dynamic  pressure  constraint  encountered  in 
many  aerospace  applications.  Once  again,  the  hard  inequality  constraint  represents  a  state 
variable  inequality  constraint-in  this  case  it  is  a  first-order  state  variable  inequality 
constraint  since: 

d*=vv  =  v(-lv2(0+<<<))  (4.29) 


depends  explicitly  on  the  control  variable  u(t). 

To  make  the  notation  compatible  with  Chapters  2  and  3,  define  the  following 
variables: 


*i(r)  =  /(0 
xi(t)  =  Ht) 
x3(t)  =  z(t) 


The  problem  to  be  solved  is  to  minimize: 

JNL=  ¥«1),1)  =  *>(1) 
subject  to  the  following  constraints: 

*0)  =  fW'WM 


or 


H*) =/2(x(t), iit),t)  =  -  ^l(r)  +  u{t) 

x3(t)=/3(x(t)A‘U)  = 


(4.30) 


(4.31) 


(4.32a) 


(4.32b) 
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with  initial  conditions 

q[x(0),0]  =  0  (4.33a) 

or 

*i(0)  =  0 
*2(0)  =  0 

*3(0)  =  0  (4.33b) 

terminal  constraints 

m[x(1),l]  =  0  (4.34a) 

or 

*i(l)-l=0 

*2(1)  =  0  (4.34b) 

and  inequality  constraint 

irf(f)-l£0=>Jtf(f)<;Y2 

2  (4.35) 

These  equations  form  the  basis  of  a  continuous  time  nonlinear  optimization  problem  with 
hard  inequality  constraints. 

The  initial  guess  at  a  nominal  control  is  shown  in  Figure  4.6.  The  nominal  control 
represent  a  bang-bang  control.  Using  this  control  in  the  numerical  integration  of  the 
system  dynamics,  the  nominal  state  trajectories  and  the  nominal  performance  can  be 
determined.  As  is  evident  from  Figure  4.7  and  Figure  4.8,  the  nominal  state  trajectories 
do  not  satisfy  the  boundary  conditions  very  well.  In  addition,  the  hard  inequality  constoaint 
is  clearly  violated  in  Figure  4.8. 
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Figure  4.6  Example  2  Nominal  Plot  of  u(t) 
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Figure  4.7  Example  2  Nominal  Plot  of  x\  (t) 
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Figure  4.8  Example  2  Nominal  Plot  of  x2 (t) 

The  square  root  sweep  method  was  applied  to  the  problem..  The  objective  of  this 
example  was  to  satisfy  the  hard  constraints,  the  boundary  conditions  and  to  improve  the 
performance.  The  data  necessary  to  compute  the  solution  to  this  problem  is  outlined 
below.  For  this  problem  B(A0  is  simply: 

10  0' 

B(N)=  o  1  0 

.0  0  lJ  (4.36) 

The  value  of  the  terminal  constraint  is  given  by: 

- 1  +  x\(N) 

m  =  x2(n) 

CjXt{N)  J  (4.37) 

It  may  be  desirable  to  not  constrain  the  performance  all  the  time.  This  may  occur  when  one 
would  like  to  improve  boundary  condition  and  constraint  satisfaction.  If  this  occurs,  the 
third  row  in  (4.36)  and  (4.37)  is  not  included. 
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Since  the  initial  state  is  entirely  fixed  and  hence  no  perturbation  5x(0)  is  allowed, 
B(0),  is  given  by 


10  0 
B(0)  =  0  1  0 
.0  0  1 


(4.38) 


The  value  of  the  initial  constraint  is  given  by 


'  0 

m=  0 
.  0 


(4.39) 


The  data  necessary  for  the  constraint  is  given  by 


H(*+  1)  =  [  0  jc2(>t  +  1 )  0  ] 


qt)=o 


A(*)  =  i*|(*+ 1)-1 


(4.40) 


The  result  of  applying  the  square  root  sweep  method  is  shown  in  Figure  4.9  - 


Figure  4.12. 


94 


Chapter  4  Simple  Applications  of  the  Square  Root  Sweep  Technique 


Chapter  4  Simple  Applications  of  the  Square  Root  Sweep  Technique 


Figure  4.11  Example  2  Solution  for  *2(0 
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Figure  4.12  Example  2  Solution  for  Cost 

The  resulting  trajectories  clearly  satisfy  the  boundary  conditions  and  the  inequality 
constraint.  The  performance  measure  has  also  been  substantially  improved.  It  should  be 
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noted  that  the  optimality  of  this  solution  has  not  been  checked.  Nonetheless,  the  technique 
has  generated  a  feasible  trajectory  while  reducing  the  cost 

4.4  Conclusion 

In  this  chapter,  two  simple  examples  have  been  used  to  demonstrate  the  viability  of 
the  square  root  sweep  method.  In  the  first  example,  it  was  found  that  the  technique  of 
thresholding  was  useful  in  obtaining  a  solution.  Thresholding  was  used  to  overcome  the 
tradeoff  between  constraint  satisfaction  and  performance  improvement.  Tht  solution 
obtained  using  the  square  root  sweep  method  closely  approximates  the  optimal  solution.  In 
the  second  example,  the  technique  has  been  used  to  obtain  a  reference  trajectory  that 
satisfies  constraints  and  boundary  conditions.  The  second  example  showed  how  the 
technique  can  be  used  to  satisfy  constraints  and  boundary  conditions  given  a  poor  nominal 
control  trajectory.  It  should  be  noted  that  the  better  the  initial  guess  the  better  the  overall 
performance.  Also,  depending  upon  the  nonlinearity  of  the  model,  a  good  initial  guess 
may  be  essential. 
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5.1  Introduction 

The  example  discussed  in  this  chapter  illustrates  the  application  of  the  square  root 
sweep  method  to  a  fairly  sophisticated  aerospace  example:  the  re-entry  of  a  lifting  glide 
vehicle.  In  particular,  the  following  features  of  the  technique  will  be  demonstrated.  First, 
the  ability  of  the  technique  to  handle  control  constraints  and  state-control  constraints  will  be 
shown.  In  the  previous  chapter,  all  of  the  hard  inequality  constraints  were  state  variable 
inequality  constraints.  Second,  the  ability  of  the  technique  to  handle  simultaneous 
constraint  violations  will  be  discussed.  The  focus  of  this  application  has  been  on 
developing  control  programs  that  satisfy  the  constraints.  The  issue  of  optimization  has  not 
been  addressed. 

5.2  Glide  Vehicle  Model 

The  problem  of  interest  in  this  chapter  is  to  compute  an  angle-of-attack  program  a(t) 
for  a  lifting  re-entry  vehicle.  The  models  used  for  this  example  are  drawn  from  [19].  The 
equations  of  motion  for  the  vehicle  are  given  by  the  following  system  of  nonlinear  ordinary 
differential  equations. 

v(t)  =  -D/m-gs\n{y(t))  (5  ^ 

y(f)  =  Umv{t)  +  (v(/]/(/?  +  h{t))  -  8/v(t)) cos  (y{t))  (52) 
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h{t)  =  v{t) sin  (it)) 

(5.3) 

The  state  variables  v(r),  y(f)  and  h(t)  represent  the  speed  of  the  vehicle,  the  vehicle  flight 

path  angle  and  the  vehicle  altitude,  respectively.  The  variable  m  represents  the  mass  of  the 

vehicle,  which  is  considered  constant  for  this  example.  The  initial  flight  condition  is  given 

by 

v(0)  =  36000  fils 

(5.4) 

y(0)=  -7.5° 

(5.5) 

h(0)  =  400000 ft 

(5.6) 

The  only  terminal  boundary  condition  imposed  on  the  system  is  that  the  flight  path  angle  be 

zero  at  a  specified  fixed  final  time  tf  =  300s. 

The  models  for  the  lift  L  and  the  drag  D  are  given  by 

L  =  Ciia)p{h)v2Sl2 

(5.7) 

D  =  Cd(cc)p(h)v2Sl2 

(5.8) 

The  models  for  the  coefficient  of  lift  Q,(a)  and  coefficient  of  drag  Q>(a)  are  given  by 

Ciioit))  =  .6sin  (2 a(r)) 

(5.9) 

Cc(a(r))  =  .27  +  1.82  sin2(a(f)) 

(5.10) 

The  model  used  for  the  atmospheric  density  p{h)  is  a  simple  exponential  model  as  shown  in 

(5.1 1).  The  acceleration  due  to  gravity  g{h)  is  modeled  by  (5.12). 

p(h(t))=  .002378exp(-lit)/22000) 

(5.11) 

«W/))  =  32.172  R2/(R  +  Htf 

(5.12) 
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where  R  represents  the  radius  of  the  earth  (/?  =  20,904,000 ft).  The  quantity  S/2m  appears 
in  several  of  the  equations  shown  above.  For  the  purposes  of  this  example,  this  quantity  is 
fixed  at  S/2m  =  .26245  ft2/ slug. 

5.3  Constraints 

In  addition  to  the  final  boundary  condition  on  the  flight  path  angle,  two  hard 
inequality  constraints  must  be  satisfied.  To  limit  the  angle  of  attack,  a  constraint  of  the 
form 


m  S  22.5° 


(5.13) 


is  imposed.  Control  constraints  of  the  form  in  (5.13)  often  arise  from  control  hardware 
limitations.  The  second  hard  inequality  constraint  is  imposed  on  the  vehicle  aerodynamic 
acceleration.  This  constraint  is  expressed  as 


VlU 

32.172m 


-5  £0 


(5.14) 


The  constraint  in  (5.14)  is  significantly  more  complicated  than  those  discussed  previously. 
Because  the  lift  and  drag  are  functions  of  both  the  control  variable  and  the  state  variables, 
both  H(Jfc  +  1)  and  C(k )  will  be  present  in  the  discrete-time  linear  quadratic  optimization 
problem. 

5.4  Results 


The  general  nature  of  this  problem  makes  it  a  very  challenging  control  problem.  The 
trajectory  of  the  vehicle  is  quite  sensitive  to  small  changes  in  the  angle-of-attack.  In 
generating  the  nominal  angle-of-attack  program,  it  was  observed  that  there  appears  to  be  a 
small  corridor  of  programs  that  yield  reasonable  results.  Typically  the  vehicle  skips  off  the 
top  of  the  atmosphere  or  plummet  to  earth.  A  solution  lying  between  these  two  extremes  is 
sought. 
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A  nominal  control  program  a(t)  for  this  problem  is  depicted  in  Figure  5.1. 


1 

< 

i 

eo 


Angls-of-Attack  (dag) 
Constraint 


Figure  5.1  Nominal  Control  Trajectory 

The  resulting  state  trajectories  for  the  nominal  control  in  Figure  5.1  are  shown  in 
Figure  5.2,  Figure  5.3  and  Figure  5.4 


Figure  5.2  Nominal  Velocity  Trajectory 
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Figure  53  Nominal  Flight  Path  Angle  Trajectory 


Figure  5.4  Nominal  Altitude  Trajectory 

Figure  5.3  shows  the  nominal  flight  path  angle.  Clearly  the  final  boundary  condition  of  a 
zero  flight  path  angle  is  not  met.  The  actual  value  of  the  final  flight  path  angle  was  found  to 
be  1.0422  degrees.  This  fact  is  also  evident  from  the  nominal  altitude  plot,  which  shows 
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that  the  climb  rate  is  not  zero.  The  nominal  angle-of-attack  shown  in  Figure  5.1  exceeds 
the  control  constraint  during  the  early  portions  of  the  trajectory.  As  the  vehicle  travels 
further  into  the  atmosphere,  the  aerodynamic  forces  on  the  vehicle  increase,  reaching  a  peak 
value  of  6.4422  at  approximately  64  seconds.  A  plot  of  the  nominal  aerodynamic 
acceleration  is  shown  in  Figure  5.5 


3> 


Aerodynamic  Acceleration 
Constraint 


Figure  5.5  Nominal  Aerodynamic  Acceleration 

The  nominal  trajectory  clearly  exceeds  the  aerodynamic  acceleration  constraint.  Comparing 
the  control  constraint  and  the  aerodynamic  acceleration  constraint  reveals  that  both  are 
violated  in  the  time  interval  58  £  t  £  62. 

To  correct  the  violations  of  the  terminal  constraints  and  the  violations  of  the  hard 
inequality  constraints,  the  square  root  sweep  method  has  been  applied.  One  iteration  of  the 
algorithm  yielded  the  flight  path  angle  plot  shown  in  Figure  5.6  and  the  altitude  plot  shown 
in  Figure  5.7. 
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Figure  5.6  Flight  Path  Angie  after  1  iteration 


Figure  5.7  Altitude  after  1  iteration 

The  results  shown  in  these  plots  are  somewhat  discouraging.  The  flight  path  angle 
undergoes  large  changes  sometimes  exceeding  180  degrees.  Also  the  Final  altitude 
achieved  for  this  trajectory  is  negative.  What  seems  to  be  occurring  is  a  conflict  between 
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the  control  constraint  and  the  aerodynamic  acceleration  constraint.  In  order  to  meet  the 
22.5°  angle-of-attack  constraint,  a  relatively  large  change  in  the  angle-of-attack  is  necessary 
during  the  early  portions  of  the  flight.  In  the  case  of  the  aerodynamic  acceleration 
constraint,  the  coefficients  of  the  state  variables  (i.e.  the  entries  of  H(ik  +  1))  are  small 
while  the  coefficient  of  the  control  variable  (i.e.  the  entry  of  C(k))  is  large.  The  result  of 
this  is  that  large  changes  in  the  values  of  the  state  variables  are  required  to  balance  the 
relatively  large  control  perturbations  necessitated  by  the  control  constraint.  Hence  the 
perturbations  of  the  state  variables  exceed  the  linearity  of  the  linear  perturbation  model.  To 
alleviate  this  problem,  the  first  iteration  is  completed  with  only  the  control  constraint  active. 
Subsequent  iterations  are  performed  with  both  constraints  active.  Using  this  approach,  a 
reasonably  solution  to  the  re-entry  problem  can  be  obtained.  The  results  of  applying  the 
square  root  sweep  algorithm  are  shown  in  Figure  5.8,  Figure  5.9  and  Figure  5.10. 


Velocity  (ft/s) 

Nominal  Velocity  (ft/s) 


Figure  5.8  Vehicle  Velocity 


105 


Chapter  5  Lifting  Re-Entry  Vehicle  Application 

'  1  Flight  Path  Angle  (deg) 

- -  Nominal  Flight  Path  Angle  (deg) 

“““““  Constraint 


Figure  5.9  Vehicle  Flight  Path  Angle 


Altitude  (ft) 

Nominal  Altitude  (ft) 


Figure  5.10  Vehicle  Altitude 

The  nominal  trajectories  are  plotted  in  the  above  figures  for  the  purpose  of  comparison. 
The  resulting  control  program  is  shown  in  Figure  5.1 1  and  the  aerodynamic  acceleration 
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constraint  is  shown  in  Figure  5.12.  The  nominal  trajectories  are  also  plotted  in  these 
figures. 

Angle -of -Attack  (deg) 

“  - -  Nominal  Angle-of-Attack  (deg) 

Constraint 


Time  (s) 


Figure  5.11  Control  Program 


Aerodynamic  Acceleration 
Nominal  Aerodynamic  Accelration 
Constraint 


Time  (•) 


Figure  5.12  Aerodynamic  Acceleration 
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5.5  Discussion 

The  resulting  trajectory  dips  slightly  further  into  the  atmosphere  increasing  the  drag 
enough  to  allow  the  flight  path  angle  to  decrease  toward  zero.  The  terminal  constraint  on 
the  flight  path  angle  is  met  by  the  trajectory  shown  in  Figure  5.9.  The  altitude  plot  in 
Figure  5.10  corroborates  this  observation.  Because  the  final  altitude  is  lower,  the  resulting 
final  vehicle  velocity  shown  in  Figure  5.8  is  slightly  lower. 

The  resulting  angle-of-attack  program,  shown  in  Figure  5.11  rides  along  the 
constraint  boundary  for  approximately  the  first  60  seconds  of  the  flight.  The  sharp  drop  in 
the  angle-of-attack  occurs  at  about  the  same  time  that  the  vehicle  hits  the  aerodynamic 
acceleration  constraint  as  shown  in  Figure  5.12.  By  rapidly  reducing  the  angle-of-attack, 
the  lift  and  drag  on  the  vehicle  are  reduced,  thus  preventing  the  aerodynamic  acceleration 
constraint  from  being  violated.  The  small  peak  in  the  angle-of-attack  near  90  seconds 
occurs  when  the  aerodynamic  acceleration  constraint  leaves  its  boundary.  The  latter 
portions  of  the  angle-of-attack  trajectory  are  left  relatively  unchanged.  The  resulting 
aerodynamic  acceleration  is  slightly  higher  during  the  final  phases  of  the  flight.  This 
increase  may  be  attributable  to  the  higher  density  that  occurs  at  the  lower  final  altitude. 

As  evidenced  by  these  results,  the  square  root  sweep  method  can  be  used  to  develop  a 
trajectory  that  satisfies  the  constraints  imposed  on  the  re-entry  of  a  lifting  glide  vehicle.  In 
order  to  obtain  satisfactory  results,  only  the  control  constraint  was  imposed  during  the  first 
iteration.  On  subsequent  iterations,  both  the  control  constraint  and  the  aerodynamic 
acceleration  constraint  were  imposed.  This  prevents  the  linearity  of  the  linear  perturbation 
model  from  being  exceeded.  A  second  approach  to  correcting  this  problem  would  be  to  not 
correct  for  the  entire  control  constraint  violation  in  the  region  where  both  constraints  are 
violated.  This  may  be  useful  as  a  general  heuristic  for  preventing  the  perturbations  from 
exceeding  the  range  of  linearity. 
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Initially,  the  constraint  violations  are  fairly  large,  as  the  square  root  sweep  method  is 
applied  the  constraint  violations  should  be  lessened.  The  impact  of  these  changes  on  the 
control  perturbations  is  to  make  them  smaller  and  smaller.  This  fact  can  be  observed  from 
equation  (3.66).  Hence  the  amount  of  improvement  becomes  less  and  less  on  subsequent 
iterations.  Therefore,  due  to  numerical  effects  and  the  smaller  control  perturbations,  it  may 
not  be  possible  to  exactly  satisfy  the  constraints.  To  prevent  unnecessary  iterations,  the 
user  needs  to  specify  when  constraint  satisfaction  is  good  enough. 

The  final  trajectory  achieved  was  highly  dependent  upon  the  initial  guess.  For 
example,  if  the  trajectory  generated  shown  in  Figure  5.6  and  Figure  5.7  is  used  as  an  initial 
guess,  the  technique  has  not  been  successfully  shown  to  converge  to  a  reasonable  solution. 
This  should  not  be  considered  as  an  indictment  of  the  square  root  sweep  method,  since  the 
problem  is  attributable  to  the  nonlinearities  present  in  the  problem. 

5.6  Summary 

In  this  chapter,  an  application  of  the  square  root  sweep  method  to  the  problem  of 
determining  an  angle-of-attack  program  for  a  lifting  glide  vehicle  that  satisfies  hard 
inequality  constraints  has  been  described.  Several  features  of  the  technique  have  been 
demonstrated.  First,  the  technique  has  been  shown  to  be  capable  of  handling  two 
constraints  simultaneously.  Although  the  first  iteration  had  only  the  control  constraint 
active,  subsequent  iterations  had  both  constraints  active.  Second,  the  ability  of  the 
technique  to  handle  combined  state-control  inequality  constraints  has  been  verified.  This 
feature  of  the  technique  had  never  before  been  shown.  Finally,  knowledge  of  the  problem 
is  essential  to  achieving  reasonable  results. 
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In  this  thesis  an  algorithm  for  determining  control  programs  that  generate  trajectories 
that  satisfy  hard  inequality  constraints  and  boundary  conditions  while  simultaneously 
optimizing  or  improving  a  performance  measure  has  been  analyzed. This  analysis  has 
included  a  thorough  derivation  of  the  algorithm,  as  well  as  the  application  of  the  algorithm 
to  a  variety  of  problems.  The  algorithm  begins  with  a  nominal  control  program  and  then 
computes  small  control  perturbations  based  on  a  linearized  perturbation  model  of  the 
underlying  nonlinear  dynamics.  The  control  perturbations  are  determined  by  solving  a 
discrete-time  linear  quadratic  optimal  control  problem  with  hard  intermediate  state-control 
constraints  (Chapter  2).  The  method  used  to  compute  the  control  perturbations  &u(k)  is 
based  upon  a  set  of  backward  recursions  for  the  sweep  parameters  that  are  developed  in 
Chapter  3. 

One  objective  of  this  thesis  was  to  extend  the  square  root  sweep  method  to  Mayer 
form  problems  in  the  calculus  of  variations.  This  objective  is  achieved  by  including  an 
additional  constraint  equation  in  the  final  boundary  condition.  The  simple  examples  in 
Chapter  4  demonstrated  that  the  technique  can  be  used  to  improve  a  Mayer  form  of 
performance  measure-thus  validating  the  technique.  A  second  objective  was  to 
demonstrate  that  the  technique  could  be  used  to  satisfy  constraints  written  in  terms  of  both 
the  state  and  control  variables.  The  aerodynamic  acceleration  constraint  present  in  the 
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lifting  glide  vehicle  re-entry  problem  is  an  example  of  this  type  of  constraint  In  Chapter  5, 
this  aspect  of  the  square  root  sweep  algorithm  has  been  shown.  Several  heuristic 
techniques  for  aiding  the  algorithm  have  been  discussed.  Specifically,  the  idea  of 
thresholding  was  developed  in  Chapter  4  and  a  method  of  avoiding  overconstraining  the 
problem  was  discussed  in  Chapter  5. 

A  significant  difficulty  with  the  square  root  sweep  method  is  the  determination  of  the 
appropriate  changes  in  the  nonlinear  performance  measure  at  each  iteration.  No 
consistently  well  performing  method  has  been  found.  The  present  solutions  were  achieved 
largely  through  a  trial  and  error  method.  In  general,  how  to  handle  this  difficulty  is  highly 
problem  dependent 

The  work  of  this  thesis  suggests  several  avenues  for  future  research.  First  it  is  highly 
desirable  that  a  method  of  handling  the  difficulty  discussed  in  the  previous  paragraph  be 
developed.  Second,  the  square  root  sweep  algorithm  offers  some  potential  for  parallel 
processing.  Specifically,  the  sweep  parameters  v(Jfc  +  1)  and  s(k  +  1)  can  be  computed 
in  parallel  with  the  sweep  parameters  W(Jk)  and  D (k).  The  computation  of  8u(*)  can  be 
computed  in  parallel  with  \j r(k  +  1). 

It  should  be  possible  to  extend  the  square  root  sweep  method  to  a  continuous-time 
setting.  One  method  of  doing  this  would  be  to  include  a  constraint  of  the  form 

+ qrWl)  +  A(»)  =  0  {61) 

in  the  continuous-time  analog  of  the  constrained  discrete-time  linear  quadratic 
optimization  problem  posed  in  Section  2.6. 

An  obvious  avenue  of  future  research  is  the  application  of  the  technique  to  more 
difficult  and  complex  example  problems.  The  technique  developed  in  this  thesis  represents 
a  general  mathematical  technique  and  has  a  wide  range  of  applications.  Possible 
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applications  would  be  robotics,  large  angle  slewing  of  flexible  spacecraft  and  path  control 
of  autonomous  vehicles. 
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